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ABSTRACT 


The  feasibility  of  controlling  the  three  link 
rectangular  and  revolute  robots  with  an  adaptive  computer 
simulation  model,  using  a  curve  following  technique,  is 
investigated. 

Both  configurations  are  tested  for  different  load 
conditions,  for  rejection  of  random  disturbances  and  for 
robustness  in  the  case  of  servo  motor  parameter  variations. 

The  interactive  nonlinear  dynamics  of  the  revolute 
robot,  such  as  coupling  inertia,  actuator  dynamics, 
centripetal  and  coriolis  forces,  are  also  investigated. 
First  a  gravity-free  environment  is  assumed  and  then  the 
robot  arms  are  tested  under  gravitational  torques. 
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Current  industrial  robot  arms  tend  to  have  been 
justified  because  of  their  untiring  nature,  predictability, 
precision,  reliability  and  ability  to  work  in  relatively 
hostile  environments.  Besides,  robots  frequently  increase 
industrial  productivity,  improve  the  overall  product 
quality,  allow  replacement  of  human  labor  in  monotonous  and 
mainly  hazardous  tasks  and  also  may  save  on  materials  or 
energy . 

The  dynamic  motion  of  a  robot  arm  is  strongly  affected 
by  mechanical  design,  physical  properties  of  the  arm  and  the 
effects  of  gravity.  Such  factors  as  joint  friction,  coupling 
inertia,  centripetal  forces,  actuator  dynamics  and  gravity 
effects  produce  an  interactive  nonlinear  dynamic  system.  In 
order  to  cope  with  the  rapidly  changing  dynamics,  a  robust 
and  flexible  control  algorithm  is  required. 

The  design  of  an  adaptive  algorithm  consists  basically 
of  combining  a  particular  parameter  estimation  technique  and 
any  feedback  control  law.  In  this  study,  the  use  of  a  curve 
following  technique  to  control  a  robot  arm  is  investigated. 
The  very  important  feature  of  this  control  scheme  is  that 
the  amplifier  is  intentionally  driven  to  saturation  so  full 
advantage  of  the  maximum  available  power  can  be  taken  while 
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in  linear  controllers  driving  the  amplifier  to  saturation 
must  be  avoided. 

First  the  three  link  rectangular  robot  is  used  as  the 
simulation  model  and  then  the  same  tests  are  applied  to  the 
three  link  revolute  robot.  The  same  control  scheme  is  used 
to  drive  all  servo  motors.  An  ideal  second  order  model  is 
chosen  to  approximate  the  servo  motor  and  the  adaptive 
algorithm  updates  the  second  order  position,  acceleration 
and  gain  constant  using  the  servo  motor  position  output. 

Each  of  these  configurations  is  tested  first  for 
different  load  conditions,  then  for  random  disturbance 
rejection  and  finally  for  robustness  in  the  case  of  slight 
variation  of  the  servo  motor  parameters.  In  all  these  tests 
the  interactive  nonlinear  dynamics  of  the  system  as  coupling 
inertia,  centripetal  forces  and  coriolis  forces  are  also 
investigated.  Initially  a  gravity-free  environment  is 
assumed  and  then  all  the  tests  are  repeated  assuming  the 
robot  arm  operating  under  gravitational  torques. 

Definitions  and  several  basic  ideas  about  robotics  and 
materials  which  are  referred  to  in  this  study,  are  included 
in  Chapter  II.  The  development  of  the  computer  simulation 
model  and  the  simulation  studies  of  this  model  are  discussed 
in  Chapter  III.  Chapter  IV  deals  with  the  development  of  the 
adaptive  algorithm  for  the  three  link  rectangular  robot  and 
Chapter  V  contains  the  modelling  as  well  as  the  equations  of 


motion  this  robot  while  the  simulation  results  are  studied 
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in  Chapter  VI.  The  modelling  and  the  equations  of  motion  of 
the  three  link  revolute  robot  are  presented  in  Chapter  VII 
and  the  development  of  the  adaptive  model  for  the  revolute 
configuration  is  presented  in  Chapter  VIII.  Chapter  IX 
contains  the  simulation  studies  of  the  revolute  robot  and 
Chapter  X  concludes  the  studies  of  both  configurations, 
indicating  also  some  areas  for  further  study.  The  detailed 
derivation  of  the  mathematical  models  for  both  rectangular 
and  revolute  robots  as  well  as  the  basic  DSL/VS  simulation 
programs  used  in  the  course  of  this  thesis  research  are 
listed  in  Appendices  A  through  K. 


A.  INTRODUCTION 

The  word  robotics  was  invented  by  the  Isaac  Asimov,  one 
of  the  best  of  the  science  fiction  writers,  to  describe  the 
science  of  dealing  with  robots.  In  one  of  his  stories  called 
'Runaround',  which  appeared  in  the  March  1942  issue  of 
'Astounding  Science  Fiction',  Asimov  propounded  the  famous 
Three  Laws  of  Robotics. 

1.  A  robot  must  not  harm  a  human  being  or,  through 
inaction,  allow  human  being  to  come  to  harm. 

2.  A  robot  must  always  obey  human  beings  unless  that  is 
in  conflict  with  the  First  Law. 

3.  A  robot  must  protect  itself  from  harm  unless  that  is 
in  conflict  with  the  First  or  Second  Law. 


B.  DEFINITION  OF  ROBOT 

As  can  be  imagined  from  the  confused  usage  of  the  word 
robot  there  is  no  international  agreement  about  definitions. 
Several  definitions  of  robot  exist  depending  on  what  a  robot 
actually  consists  of.  Some  of  the  most  commonly  used 
definitions  are: 

1.  A  robot  is  a  mechanical  positioning  system. 

2.  A  robot  is  a  pick  and  place  device. 

3 .  The  popular  conception  is  of  a  mechanical  man  capable 
of  carrying  out  tasks  that  a  human  might  do  and 
displaying  some  capability  for  intelligence. 


4.  A  robot  is  a  reprogrammable,  multifunctional  manipula¬ 
tor  designed  to  move  material,  parts,  tools  or  specia¬ 
lized  devices  through  variable  programmed  motions  for 
the  performance  of  a  variety  of  tasks. 

C.  THE  ROBOT  ARM 

Although  mobile  robots  one  day  should  be  common,  for  the 
present  the  state  to  which  the  industrial  robot  has  evolved 
is  best  referred  to  as  a  robot  arm.  Most  of  them  are 
essentially  a  mechanical  arm  fixed  somewhere  or  to  another 
machine,  fitted  with  a  special  end  -  effector  which  can  be 
either  a  gripper  or  some  sort  of  tool.  The  arm  moves,  by 
means  of  hydraulic,  electric  or  pneumatic  actuators,  in  a 
sequence  of  preprogrammed  motions  under  the  direction  of  a 
controller  which  these  days  is  almost  always  microprocessor 
based  and  senses  the  position  of  the  arm  by  monitoring 
feedback  devices  on  each  joint.  The  range  of  positions 
which  the  arm  can  reach  is  called  work  volume  or  work  space 
or  work  envelope. 

D.  DIFFERENT  ARM  CONFIGURATIONS 

The  essential  role  of  a  robot  arm  is  to  move  a  gripper 
or  tool  to  given  orientations  at  a  given  set  of  points. 
Mathematically,  to  be  able  to  orient  an  object  in  any  way  at 
any  point  in  space  requires  an  arm  with  six  degrees  of 
freedom,  as  depicted  in  Figure  2.1.  Three  translational 
(forward/back,  right/left,  up/down)  to  reach  any  point,  and 
three  rotational  (pitch,  roll,  yaw)  to  get  any  orientation. 


The  work  envelope  of  an  arm  varies  in  shape  depending  on 
the  actual  configuration  chosen  for  the  design  of  the  arm. 
One  common  structural  classification  of  robot  arms  is 
according  to  the  coordinate  system  of  the  three  major  (the 
translational)  axes  which  provide  the  vertical  lift  motion, 
the  in/ out  reaching  motion,  and  the  rotational  or  traversing 
motion  about  the  vertical  lift  axis  of  the  robot.  Such  a 
classification  can  distinguish  between  four  basic  types. 

1.  Cartesian  (Rectangular^  Coordinate  Robot 

This  type  of  robot,  shown  in  Figure  2.2,  is 
configured  with  the  mutually  perpendicular  traversing  axes. 
Each  part  of  the  robot’s  arm  slides  at  right  angles  to  the 
previous  part.  So  the  robot  can  reach  any  part  of  volume 
bounded  by  the  length  of  the  separate  sections  of  the  arm. 
This  configuration  is  clearly  ideally  suited  for  direct 
usage  of  the  mathematical  cartesian  (or  rectangular) 
coordinate  system. 


In  this  type  of  robot,  shown  in  Figure  2.3,  the 
horizontal  arm  can  move  in  and  out  parallel  to  the  base, 
can  move  up  and  down  the  vertical  column,  and  the  whole  base 
can  rotate  around  the  vertical  axis,  so  the  end  of  the  arm 
sweeps  out  a  work  envelope  which  is  a  partial  cylinder.  This 
configuration  is  clearly  ideally  suited  for  direct  usage  of 
the  mathematical  cylindrical  coordinate  system. 
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3 .  Spherical  ( Polar )  Coordinate  Robot 

The  robot  in  Figure  2.4  has  an  arm  capable  of  linear 
motion  in  and  out.  It  is  supported  by  two  other  sections, 
one  that  rotates  around  the  base  and  one  that  rotates  about 
an  axis  perpendicular  to  the  vertical  through  the  base.  This 
arm  can  reach  almost  anywhere  in  a  volume  bounded  by  an 
outer  and  an  inner  hemisphere.  The  radii  of  the  two 
hemispheres  correspond  to  the  maximum  and  minimum  extensions 
of  the  linear  section,  respectively.  This  corresponds  to  the 
mathematical  spherical  (or  polar)  coordinate  system. 

4 .  Revolute  Coordinate  (Jointed  Arm)  Robot 

An  example  of  this  fourth  class  of  robot,  sometimes 
known  as  an  anthropomorphic  robot,  is  shown  in  Figure  2.5. 
It  consists  of  rotary  joints  called  the  'shoulder'  and  the 
'elbow' (corresponding  to  the  human  arm)  all  mounted  on  a 
'waist'  consisting  of  a  rotary  base  which  provides  the  third 
degree  of  freedom.  This  revolute  (or  jointed  arm) 
configuration  has  the  advantage  of  having  a  very  large  work 
envelope  for  its  size,  so  minimizing  floor  space 
requirements . 

E.  TYPE  OF  CONTROL 

1 .  Point-to-Point  Robots 

Point-to-point  robots  are  able  to  move  from  one 
specified  point  to  another  but  cannot  stop  at  arbitrary 
points  not  previously  designated.  They  are  the  simplest  and 
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least  expensive  type  of  robot.  Stopping  points  are  often 
just  mechanical  stops  that  must  be  adjusted  for  each  new 
operation.  Point-to-point  robots  driven  by  servos  are  often 
controlled  by  potentiometers  set  to  stop  the  robot  arm  at  a 
specified  point. 


Continuous -path  robots  are  able  to  stop  at  any 
specified  number  of  points  along  a  path.  However,  if  no  stop 
is  specified,  they  may  not  stay  on  a  straight  line  or 
constant  curved  path  between  specified  points.  Every  point 
must  be  stored  separately  in  the  memory  of  the  robot. 

3 .  Control led- Path  (Computed  Trajectory)  Robots 
Control  equipment  on  controlled-path  robots  can 

generate  straight  lines,  circles,  interpolated  curves,  and 
other  paths  with  high  accuracy.  Paths  can  be  specified  in 
geometric  or  algebraic  terms  in  some  of  these  robots.  Only 
the  start  and  finish  coordinates  and  the  path  definition  are 
required  for  control. 

4 .  Servo  versus  Non-Servo  Robots 
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F.  CATEGORIES  OF  SERVO  SYSTEM  OPERATION 


In  servo  control,  each  joint  is  controlled  by  an 
independent  position  servo,  with  joints  moving  from  position 
to  position  independently.  There  are  three  possible 
categories  of  motion. 


Joints  are  activated  one  at  a  time  while  the  other 
axes  are  held  fixed.  This  procedure  simplifies  control,  but 
the  operating  time  is  longer  than  it  would  be  if  all  joints 
operated  at  the  same  time.  It  provides  better  path  control 
because  there  is  no  interaction  between  joint  movements. 

2.  Uncoordinated  Joint  Control 


All  joints  are  allowed  to  move  together,  so  that 
they  follow  a  path  determined  by  the  relative  speeds  of  each 
joint  movement.  There  is  no  coordination  between  joints. 
Thus,  prediction  of  the  end-of-arm  path  is  difficult  or 
impossible,  since  varying  arm  loads  or  varying  friction 
might  change  the  joint  velocities  in  a  random  way. 
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Coordinated  Joint  Control 


This  is  the  most  useful  type  of  point-to-point 
control  but  requires  more  control  equipment.  Individual 
joint  motions  are  coordinated  to  reach  their  endpoints 
simultaneously . 
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6.  DRIVE  MECHANISM 


Examples  of  linear  drives  are  the  x,  y  and  z  drives 
of  rectangular  coordinate  system,  the  vertical  and 
horizontal  positioner  of  cylindrical  systems,  and  the  radial 
drive  on  spherical  systems.  Linear  motion  may  be  generated 
directly  by  a  hydraulic  or  pneumatic  piston  in  a  cylinder  or 
by  conversion  of  rotary  motion  to  linear  motion  through  rack 
and  pinion  gearing,  lead  screws,  worm  gears,  or  ball  screws. 

2 .  Rotary  Drives 

Most  electric  motors  and  servo  drive  motors  generate 
rotary  motion  directly,  but  often  at  a  lower  torque  and 
higher  rotational  velocity  than  is  desired.  It  is  necessary, 
therefore,  to  use  some  kind  of  gear  train,  belt  drive,  or 
other  mechanism  to  convert  the  available  high  speed  to  lower 
speed  with  greater  torque.  There  are  also  some  cases  in 
which  linear  hydraulic  cylinders  or  pneumatic  cylinders  are 
used  as  a  power  source,  so  that  the  linear  motion  must  be 
converted  to  rotary  motion.  The  main  options  available  for 
consideration  are  gear  trains,  timing  belt  drives,  harmonic 
drives,  rotary  hydraulic  motors  and  the  recently  developed 
direct-drive  torque  motors. 


A.  INTRODUCTION 

The  simulation  model  chosen  was  a  servo  motor  with  a 
curve  following  velocity  loop,  identical  with  the  one  used 
in  Ref.  1  and  Ref.  2.  The  velocity  curve  following  scheme  is 
a  non  linear  adaptive  scheme  that  has  been  used  mainly  in 
disk  drive  systems  [Ref.l],  but  recent  studies  [Ref. 2]  have 
justified  that  the  same  scheme  can  also  be  used  to  control  a 
rigid  robot  arm. 

Although  in  linear  controllers  driving  the  amplifier  to 
saturation  limits  must  be  avoided,  in  the  case  of  the  curve 
following  scheme  the  amplifier  is  intentionally  driven  to 
saturation.  This  is  a  very  important  feature  because  full 
advantage  of  the  motor's  capabilities  can  be  taken. 
Furthermore  this  system  is  simple  and  easily  implemented  in 
a  microprocessor  [Ref.3]. 

For  a  given  step  position  command  this  model  operates  in 
two  modes:  An  initial  full  acceleration  mode  and  a  curve 
following  mode.  The  equivalent  transfer  function  of  the 
servo  motor  is  [Ref. 4]  : 


where 

©  =  Angular  position  of  the  shaft 

V  =  Applied  dc  voltage 

Kv  =  Back  emf  constant 

K-t  =  Torque  constant 

J  =  Total  inertia 

R  =  Armature  resistance 

L  =  Armature  inductance 

When  the  inertia  of  the  robot  arm  is  added  to  the  motor 
inertia,  the  mechanical  pole  of  the  motor  becomes  very  small 
and  since  the  ratio  L/R  is  a  small  number,  the  electrical 
pole  of  the  motor  can  be  neglected.  With  the  above 
approximations  the  transfer  function  of  the  robot  arm  and 
motor  becomes  approximately: 


V(s)  s 


Since  in  the  mass  production  of  such  systems  there  would 
be  substantial  tolerances  on  the  physical  motors,  a  more 
complete  motor  model  is  not  justified. 

The  second  order  model  with  a  curve  following  velocity 
loop  is  shown  in  Figure  3.1.  If  the  curve  to  be  followed  is 
chosen  to  be  the  deceleration  curve  for  the  idealized  motor, 
the  model  will  be  a  practical  application  of  bang-bang 
control  [Ref. 5]. 
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Figure  3 . 1  Block  Diagram  of  the  Model 


B.  DESCRIPTION  OF  THE  MODEL 

When  a  step  position  command  is  applied  to  the  model, 
the  error  signal  (E)  will  produce  a  velocity  command  input 
(X)  .  This  signal  saturates  the  amplifier  and  full  forward 
drive  signal  is  applied  to  the  motor  (full  acceleration 
mode) .  As  the  error  signal  decreases,  the  velocity  command 
is  reduced  and  when  it  becomes  equal  to  the  velocity 
feedback  signal  (KC)  the  system  changes  into  curve  following 
mode  and  follows  the  curve  down  until  the  desired  position 
is  reached. 

For  the  system  to  be  able  to  follow  the  curve  to  the 
desired  position  under  different  loading  conditions  a  proper 
curve  must  be  chosen.  Since  a  parabolic  curve  approximates 


-■  s.  - 


the  deceleration  curve  of  an  ideal  motor,  it  was  chosen  as 
the  curve  to  be  followed.  To  enable  the  system  to  follow  the 
curve  accurately  the  curve  should  be  below  the  motor 
deceleration  curve.  This  is  accomplished  by  proper  selection 
of  the  gain  constant  K1  which  reshapes  the  motor  curve.  In 
this  case  the  selected  value  for  the  gain  constant  K1  was 
0.6. 

It  has  been  shown  [Ref. 3]  that  the  equation  of  the  curve 
was  derived  from  the  idealized  motor  equations  as  follows: 


C  =  Km  V 


m  vsat 


C  dt  -  Km  Vsat  t  +  C (0 )  ,  (C(0)  =0) 


C  -  C  dt  =  1/2 (Km  Vsat  t2)  +  C ( 0) 


From  equation  3.4 


Km  vsat 


and  substituting  into  equation  3 . 5 


2  Km  vsat 


(3.3) 


(3.4) 


(3.5) 


(3.6) 


(3.7) 


For  deceleration  from  initial  conditions  with  the  input  R=0 


C  =  -  E 


C  =  - 


(3.8) 


(3.9) 
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Substitution  of  equation  3.8  and  3.9  into  equation  3.7  gives 


£ 


£2 

2  Km  Vsat 


(3.10) 


or 


V" 


2  %  Vsat 


(3.11) 


Letting 

A  -  V  2  Km  Vsat  (3.12) 


and 

♦  • 
X  =  E 


(3.13) 


equation  3.11  becomes 


X 


velocity  command  input 


(3.14) 


Therefore,  if  the  parameter  A  is  initially  calculated, 
then  the  commanded  velocity  curve  can  be  generated  by 
continuous  multiplication  of  the  calculated  parameter  A  with 
the  square  root  of  the  error  signal  (E) . 

The  servo  motor  gain  constant  Km  is  determined  by  the 
transfer  function  of  the  motor  that  the  simulation  model 
will  control,  considering  the  motor  parameters  and  the 
effective  inertia  seen  by  the  motor. 
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For  the  three  link  rectangular  robot  the  chosen  values 
of  Km  were  (this  choice  will  be  justified  in  Chapter  IV) 
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Kml  =  59*29  rad/volt 
Km2  =  90.25  rad/volt 
Km3  =  77.44  rad/volt 
and  for  the  three  link  revolute  robot  the  chosen  values  of 
Km  were  (this  choice  will  be  justified  in  Chapter  VII) 

Kml  =  0.4225  rad/volt 
Km2  =  9 . 4225  rad/volt 
Km3  ■  4 . 0  rad/volt 

The  saturation  limits  (iVsa^)  of  the  amplifiers  are 
determined  by  available  power  supply  voltage,  servo  motor 
parameters,  mechanical  design  of  the  arm,  working  conditions 
and  curve  constant  K^.  This  limit  was  arbitrary  chosen  as 
±150  volts  for  all  actuators,  under  different  loading  and 
working  conditions. 

The  value  of  the  gain  constant  K2  must  be  chosen  such 
that  saturation  of  the  amplifier  occurs  for  small  signals, 
allowing  the  amplifier  to  operate  as  a  switch  providing  full 
forward  and  reverse  drive  signals  to  the  motor  in  the  curve 
following  mode.  To  effectively  saturate  the  amplifier,  K2 
was  given  the  value  of  10,000. 

The  gain  of  the  velocity  feedback  channel  (K)  is  chosen 
such  that  X  =  KC  when  the  simulation  motor  velocity  (C)  is 
at  the  desired  speed  for  a  given  step  position  input.  From 
Figure  3.1,  for  deceleration  from  initial  conditions  with 
R=0,  and  because  of  equation  3.9 


Thus,  the  gain  of  the  velocity  feedback  channel  (K) 
should  be  equal  to  K^.  From  simulation  studies,  the  best 
curve  following  was  achieved  when  K  was  given  the  value  of 
unity  (K  =  1)  . 

C.  SIMULATION  STUDIES  OF  THE  MODEL 

To  demonstrate  the  curve  following  ability  of  this 
scheme,  the  model  was  simulated  using  DSL/VS.  Appendix  A 
lists  the  DSL  simulation  program  used  for  these  studies.  Km 
was  set  to  59.29,  a  value  which  was  found  for  the  parameter 
Kml  of  the  three  link  rectangular  robot.  All  the  other 
parameters  and  variables  used  in  the  program  are  as 
previously  discussed  in  this  chapter.  The  constant  CF  is  a 
factor  which  converts  the  rotary  motion  to  linear  motion. 
The  parameter  RO  represents  the  radius  of  the  pinion  used 
(to  be  discussed  'in  chapter  IV)  and  was  chosen  to  be  0.5  in. 

The  phase  plane  trajectories  (velocity  CX  versus 
position  CX)  for  a  step  position  command  of  1  inch  are  shown 
in  Figure  3.2.  The  figure  shows  that  the  linear  velocity  of 
the  model  increases  until  the  commanded  velocity  (X)  is 
reached.  Then  the  model  velocity  follows  the  curve  down  to 
the  commanded  position.  The  step  response  of  the  model, 
depicted  in  Figure  3.3,  shows  good  performance  of  the  model. 
Now  since  a  simulation  model  has  been  found,  use  of  this 


model  to  control  the  servo  motors  of  a  three  link  robot  arm 
(rectangular  and  revolute)  will  be  investigated. 


Figure  3.3  Step  Response  of  the  Model 
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A.  INTRODUCTION 

The  second  order  model  developed  in  Chapter  III  will  be 
operated  open  loop  to  control  its  output,  C,  and  bring  it  to 
the  commanded  position.  But  this  is  not  the  desired  solution 
because  what  is  really  wanted  is  to  control  the  output 
position,  CR,  of  the  real  motor  which  must  be  driven  to  the 
commanded  location.  Due  to  the  different  dynamics  of  the 
ideal  and  the  real  motor  there  will  be  always  a  discrepancy 
between  their  outputs  C  and  CR.  To  overcome  this  undesired 
situation,  the  model  states  and  the  gain  constant  must  be 
adapted  in  a  such  a  way  that  the  ideal  motor  imitates  the 
real  one. 

In  this  chapter,  the  selection  of  the  servo  motors  is 
first  discussed  and  then  the  adaptive  algorithm  to  update 
the  model  states  and  gain  constant  is  obtained. 


B.  SELECTION  OF  POSITIONING  SERVO  MOTORS 


The  selected  real  motor  is  a  permanent  magnet  motor 
drive,  which  is  widely  used  in  industrial  robots,  and  its 
data  are  listed  in  Table  1  [Ref. 6].  This  is  a  rotary  motor 
and  since  a  linear  motion  is  required  for  the  rectangular 
coordinate  robot  arm,  the  rotary  motion  is  converted  to 
linear  through  rack  and  pinion  gearing. 
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TABLE  1 

PARAMETRIC  DATA  FOR  JOINT  SERVO 

MOTOR 

Total  Inertia  Jm 

0.033 

oz-in-sec2/rad 

Torgue  Constant  Kt 

14.4 

oz-in/amp 

Back  emf  Constant  Kv 

0.1012 

volts-sec/rad 

Damping  Coefficient  Bjj 

0.04297 

oz-in-sec/rad 

Ave.  Terminal  resistance  R 

0.91 

ohms 

Armature  Inductance  L 

100.0 

^-henries 

A  rack  is  a  long  metal  bar  that  has  gear  teeth  cut 
across  its  width.  A  pinion  gear  is  an  auxiliary  smaller, 
round  gear  placed  so  that  its  teeth  can  mesh  with  the  teeth 
of  the  rack.  Usually  the  rack  is  fixed  in  place.  As  the 
pinion  gear  rotates,  it  moves  an  attached  carriage  linearly 
along  the  rack.  The  rotary  motion  of  the  pinion  gear  is 
converted  to  linear  motion  of  the  carriage,  as  shown  in 
Figure  4.1  [Ref. 7].  Support  for  the  carriage  is  provided  by 
a  guide  rod  or  guide  way.  For  these  studies  a  pinion  gear  of 
radius  1.0  inch  was  chosen. 

The  same  motor  drive  is  used  for  the  three  arm  joints 
with  the  same  power  supply.  To  calculate  the  transfer 
function  of  the  servo  motors,  the  effective  joint  inertias 
must  be  first  calculated  as  follows 
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Figure  4.1  Rack-and-Pinion  Gearing 

JEF1  =  Mlr2+Jml 
jEF2  =  M2r  +Jm2 
JEF3  "  M3r2+Jm3 

Substitution  of  the  parameters  results  in 


(4.1) 

(4.2) 

(4.3) 


JEF1  “  0.167  oz.in.sec2 
JEF2  =  0.043  oz.in.sec2 
JEF3  =  0.100  oz.in.sec2 


Plugging  these  results  and  the  parameters  from  Table  1  into 
equation  3.1,  the  transfer  functions  of  the  three  servo 
motors  become 


9.88 

(s)  =  -  rad/volt 

s(s/9. 589+1) (s/9100+1) 

9.88 

G2(s)  =  -  rad/volt 

s(s/37. 242+1) (S/9100+1) 


(4.4) 


(4.5) 


<»3  (s) 


S(S/16. 014+1) (S/9100+1) 


rad/volt 


(4.6) 


These  results  were  used  to  determine  the  gains  Km  for 
the  model  motors.  Figures  4.2,  4.4  and  4.6  show  the  Open 

Loop  Bode  diagrams  of  the  three  real  motors  transfer 
functions  respectively.  From  these  diagrams  it  can  be 
observed  that  the  -40  db/decade  slope  intersects  the  0  db 
axis  at(Ji=7.7  rad/sec  for  the  J0INT1  motor, £J2“9*5  rad/sec 
for  the  JOINT2  motor  and  0J3=8.P  rad/sec  for  the  JOINT3 
motor.  Using  the  above  frequencies  as  gain  crossover 
frequencies  for  the  second  order  ideal  motors,  we  calculate 

Kml  *  7*72  *  59.29  rad/volt 
Km2  “  9.52  *  90.29  rad/volt 
Km3  *  8.82  =*  77.44  rad/volt 

The  Open  Loop  Bode  diagrams  of  the  K^/s2  ideal  motors 
are  shown  in  Figures  4.3,  4.5  and  4.7. 

C.  ALGORITHM  TO  UPDATE  THE  ADAPTIVE  MODEL 

The  adaptive  model  depicted  in  Figure  4.8  is  used  to 
drive  a  joint  servo  motor.  The  same  scheme  is  used  for  each 
of  the  three  servo  motors.  The  output  of  the  saturating 
amplifier  (V)  is  common  to  both  the  model  and  the  servo 
motors.  The  output  of  the  servo  motor  (CR)  is  measured,  at 
prespecified  time  intervals,  and  fed  into  the  Adaptive 
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gure  4.3  Open  Loop  Bode  Plot  of  the  Kml/s2  Moto 
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Algorithm  which  updates  the  velocity  (C)  and  position  (C) 
of  the  model,  and  the  gain  parameter  (Km)  .  Km  is  not  kept 
constant  during  the  arm's  motion  but  is  adjusted  in  such  a 
way  as  to  account  for  the  inertia  reflected  back  from  the 
arm.  For  the  above  reasons  an  estimate  of  the  servo  motor 
gain  (Km)  as  well  as  the  linear  velocity  (CR)  of  the  servo 
motor  must  be  determined.  Two  main  considerations  that  must 
be  taken  into  account  in  these  calculations  are:  [Ref.l] 

1.  The  calculations  must  be  reasonably  accurate  to  allow 
the  model  states  to  approximate  the  trajectory  of  the 
servo  motor. 

2.  Since  the  updating  of  the  model  states  and  gain 
constant  in  minimum  time  is  required,  the  calculations 
must  be  kept  as  simple  as  possible. 

There  are  several  different  methods  for  these 
calculations.  Since  the  method  described  in  Ref.  1  best 
satisfies  both  of  the  above  requirements,  it  is  used  for  the 
calculations. 

From  equation  3 . 5  solving  for  K™ 


Km  = 


t2 


(4.7) 


For  discrete  time  intervals 


Km  = 


vsat  (NT)2 


(4.8) 


where  N  is  the  number  of  samples  and  T  is  the  sampling 
interval . 
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Letting  C  =  CR 


2  CR 

Km  =  - —  (4.9) 

vsat  (NT)2 

Equation  4.9  is  valid  only  for  the  full  acceleration  portion 
of  the  move  where  the  acceleration  of  the  servo  motor  is 
constant.  Therefore,  the  model  gain  Km  is  updated  until  the 
velocity  of  the  servo  motor  reaches  the  velocity  curve 
following  portion  of  the  move. 

The  estimated  velocity  of  the  servo  motor  is  [Ref.l] 

2 [CR(N) -CR(N-l) ] 

CR  (N)  - - CR(N-l)  (4.10) 

T 

This  calculation  gives  an  estimate  of  the  velocity  as  soon 
as  the  last  position  of  the  servo  motor  is  known,  and 
requires  the  storage  of  the  last  position  measurement  [CR(N- 
1) ]  and  the  last  estimated  velocity  [CR(N-l)].  After  2 
samples  of  position  are  known,  the  stored  velocity  CR(N-l) 
can  be  recalculated  as 

CR (N)  -  CR (N-2 ) 

CR(N-l)  =  -  (4.11) 

2T 

If  the  model  switches  from  full  acceleration  to  full 
deceleration  (curve  following  mode)  between  samples  of 
position,  the  velocity  given  by  equation  4.10  is  not  self- 
correcting  until  two  position  samples  can  be  obtained  after 


switching.  To  remove  this  discrepancy,  the  algorithm  detects 
the  switching  time  and  stores  the  present  value  of  the  model 
velocity  as  CR(N-l)  to  be  used  in  the  next  calculation. 

As  has  been  discussed  earlier  in  this  chapter,  generally 
CR  does  not  duplicate  C,  because  of  the  different  dynamics 
between  model  and  real  motor.  However,  if  C  and  CR  start 
with  the  same  values,  and  if  they  have  identical  initial 

•  k 

derivatives  C  and  CR,  then  it  is  expected  that  CR  and  CR 

« 

will  not  deviate  substantially  from  C  and  C  over  a  very 
short  time  interval.  If  C  and  C  are  reset  at  the  end  of  this 
interval  and  given  the  actual  values  of  CR  and  CR,  then  both 
model  and  motor  start  the  next  interval  with  the  same 
states.  Also  the  gain  parameter,  Km,  is  updated  during  the 
full  acceleration  portion  of  the  move.  [Ref. 8] 

In  the  simulation  of  this  model  fixed  step  integration 
is  used  as  the  easiest  numerical  integration  method.  The 
integration  step  size  parameter,  DELT,  is  chosen  to  be  1/5 
of  the  sampling  interval  so  that  5  integration  steps  are 
completed  between  two  successive  updates  of  the  adaptive 
model.  The  model  velocity  error  signal,  XE,  is  computed  at 
each  integration  step  and  is  sent  to  the  amplifier. 
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A.  INTRODUCTION 

In  this  chapter  the  dynamic  equations  of  the  three  link 
rectangular  robot  are  first  developed  and  then  solved  in 
order  to  obtain  the  equations  of  motion.  Although  using 
Lagrangian  mechanics  the  dynamic  equations  of  any  system  can 
be  obtained  in  a  relatively  simple  manner,  the  Newton-Euler 
formulation  of  equations  of  motion  is  more  convenient  in  the 
case  of  the  cartesian  coordinate  configuration  and  for  this 
reason  is  preferable  over  the  Lagrangian  method. 

Since  the  motion  of  each  link  is  independent  of  the 
motion  of  the  other  two  links,  there  is  no  inertial  coupling 
between  the  three  joints.  Besides,  the  only  link  affected  by 
the  gravitational  acceleration  is  the  vertical  one  while  the 
other  two  links  move  in  directions  perpendicular  to  the 
direction  of  gravity  and  therefore  are  not  affected  at  all. 


B.  MODEL  DEVELOPMENT 

The  three  link  rectangular  robot  to  be  tested  is 
illustrated  in  Figure  2.2.  The  rotary  motion  of  the  three 
joint  servo  motors  is  converted,  through  rack  and  pinion 
gearing  of  radius  r,  to  linear  motion,  and  the  motions  of 
the  three  links  are  constrained  to  the  x,  y  and  z  directions 
respectively.  The  link  moving  along  the  y  axis  has  a 


gripper,  as  end  effector,  which  can  carry  a  load.  The  mass 
of  the  servo  motors  is  denoted  1%  and  the  masses  of  the 
three  links  m1#  m3  respectively.  The  total  masses  seen 
by  each  servo  motor  are  denoted  Mj_,  M2/  and  M3 .  F^,  F2,  F3 
represent  the  forces  acting  on  the  three  links  in  the 
directions  of  motion,  and  T^ ,  T2,  T3  represent  the  external 
driving  torques  to  the  joints. 

All  joints  can  move  simultaneously,  so  the  end-of-arm 
follows  a  path  determined  by  the  relative  speeds  of  each 
joint  velocity.  However,  prediction  of  this  path  is 
difficult  because  varying  arm  loads  or  varying  friction 
change  the  joint  velocities  in  a  random  way. 


C.  EQUATIONS  OF  MOTION 

Applying  the  Newton-Euler  method,  the  motion  of  a  rigid 
body  can  be  decomposed  into  the  translational  motion  of  an 
arbitrary  point  fixed  to  the  rigid  body,  and  the  rotational 
motion  of  the  rigid  body  about  that  point.  The  dynamic 

equations  of  a  rigid  body  can  also  be  represented  by  two 
equations:  one  which  describes  the  translational  motion  of 
the  centroid,  while  the  other  describes  the  rotational 

motion  about  the  centroid.  The  former  is  Newton's  equation 
of  motion  for  a  mass  particle,  and  the  latter  is  called 

Euler's  equation  of  motion. 

In  the  specific  case  of  rectangular  coordinate  robot 
there  is  no  rotational  motion,  therefore  the  dynamic 
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equations  are  obtained  applying  only  Newton's  equation.  If 
vci  Is  the  linear  velocity  of  the  centroid  of  the  link  i, 
then  the  inertial  force  is  given  by  -MiVci,  where  is  the 
total  mass  seen  by  the  servo  motor  i.  Then,  the  equation  of 
motion  for  the  link  i  is  obtained  by  adding  the  inertial 
force  to  the  static  balance  of  forces  so  that 

F^_ 1 1  ^  i+i+Mj_g— M  £Vq  =  0  ,  for  i— 1,2,3  (5.1) 

where  F^-i^  and  Fi^+1  are  the  coupling  forces  applied  to 
link  i  by  links  i-l  and  i+1,  respectively,  and  g  is  the 
acceleration  of  gravity. 

In  the  case  of  rectangular  coordinate  robot  due  to 
absence  of  any  inertial  coupling  between  the  joints, 
equation  5.1  becomes 

Fi+MjC(-MjVci  =  0  ,  for  i=l,2,3  (5.2) 

where  F^  is  the  force  applied  to  the  link  i  by  the  joint 
motor  i,  in  the  direction  of  motion  of  the  link  i.  The  total 
torque  about  the  axis  of  rotation  of  the  motor  i  is  given 
then  by  the  equation 

Tj.  =  Fir+JjniGi  (5.3) 


where  r  is  the  radius  of  the  pinion  gearing,  is  the 

inertia  of  motor  i,  and  0^  is  the  angular  acceleration  of 
the  motor's  i  shaft. 
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Solving  equations  5.2  and  5.3  for  the  three  links,  the 
following  second-order  nonlinear  differential  equations  have 
been  derived  and  used  as  the  basic  mathematical  model  for 
the  simulation  of  the  three  link  rectangular  robot.  The 
derivation  of  these  equations  is  shown  in  details  in 
Appendix  B. 


T1  -  (Mir2+Jmi) 9i 

(5.4) 

T2  “  (M2r2+Jm2)  6*2 

(5.5) 

T3  =  ( M3  r 2  + Jm3 ) 8 3 -M3  gr 

(5.6) 

where 

Mi  -  mi+m2+m3+2mm+Load 
m2  =  m2+Load 
m3  =  m2+m3+mm+Load 
mi  =  0.082  oz/in/sec2 
m2  =  0.041  oz/in/sec2 
m3  =  0.041  oz/in/sec2 
mm  =  0.186  oz/in/sec2 
r  -  0.5  in 
g  =  386.4  in/sec2 

In  these  equations,  terms  of  the  form  Mir2+Jmi  are  known 
as  the  effective  inertia  at  joint  i. 


A.  PLANNING  THE  SIMULATION  STUDIES 

The  already  developed  model  for  the  three  link  robot  is 
tested  for  the  Voltage  Source  Drive.  First  a  gravity-free 
environment  is  assumed  and  the  model  is  tested  for  different 
load  conditions,  for  rejection  of  a  disturbance  applied 
before  and  after  steady  state  condition,  and  finally  for 
robustness  in  the  case  of  motor  parameters  variation.  Then 
the  same  tests  are  repeated  but  in  this  case  gravitational 
torques  are  taken  into  account. 

The  tested  arm  is  a  point-to-point  arm  and  the  unit  step 
input  command  is  applied  in  all  joints  simultaneously  for 
comparison  reasons.  The  motions  of  the  three  joints  are 
independent  of  each  other,  and  the  path  that  the  end  of  the 
gripper  follows  depends  on  the  relative  speeds  of  each  joint 
movement.  In  this  thesis  a  free  work  space  is  assumed  for 
the  tested  arm  so  that  obstacle  avoidance  is  not  a 
consideration . 

For  every  simulation  program  three  plots  are  obtained: 

1 .  Phase  plane 

2 .  Step  response 

3.  Error  versus  time 

In  all  phase  plane  plots  the  model  linear  velocity,  C,  and 
the  actual  system  linear  velocity,  CR,  are  plotted  as 


ordinates  versus  the  system  linear  position,  CR.  In  step 
response  plots  the  linear  positions  of  the  model,  C,  and  the 
actual  system,  CR,  are  plotted  as  functions  of  time. 
Although  the  linear  position  and  velocity  of  the  actual 
system  can  not  be  observed  in  the  above  plots,  they  are 
available  in  simulations  and  are  used  to  check  the  validity 
of  the  adaptive  algorithm.  Finally  in  the  last  group  of 
plots  the  error  between  the  commanded  and  the  actual 
position  of  the  system  is  plotted  also  as  a  function  of 
time. 


B.  SIMULATION  STUDIES  OF  THE  ADAPTIVE  SYSTEM 
1.  Gravity-free  Environment 

The  adaptive  system  in  a  gravity-free  environment  is 
tested  first  for  different  load  conditions.  Then  the 
capability  of  the  system  to  reject  a  disturbance,  which  is 
applied  in  any  time  instant,  is  tested  and  finally  the 
behavior  of  the  system  is  examined  for  a  slight  variation  of 
servo  parameters. 

a.  Different  Load  Conditions 

The  adaptive  system  is  tested  first  without 
carrying  any  load  and  then  with  different  load.  The  DSL/VS 
simulation  program  used  for  these  tests  is  listed  in 
Appendix  C.  Figures  6.1,  6.2  and  6.3  are  the  phase  plane 
plot,  the  step  response  and  the  error  curve,  respectively, 
for  the  unloaded  arm. 
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Figure  6.1  shows  that  the  adaptive  model  and  the 
servo  motor  have  good  curve  following  characteristics 
for  the  three  joints.  It  shows  also  that  the  linear 
acceleration  of  the  three  links  is  inversely  proportional  to 
their  masses.  So  the  link  moving  along  the  y-axis,  which  is 
the  smallest  one,  moves  faster  than  the  other  two  links  and 
is  the  first  one  that  reaches  the  commanded  velocity  curve, 
while  the  link  moving  along  the  x-axis,  which  is  the  largest 
one,  is  the  last  one  that  reaches  the  velocity  curve.  The 
link  moving  along  the  z-axis  moves  with  an  intermediate 
velocity.  Finally  all  links  track  the  commanded  velocity 
curve  down  to  the  desired  position. 

The  step  response  curves  of  Figure  6.2  show  the 
three  model  and  servo  motors  tracking  together  to  steady 
state  condition.  The  y-axis  link  reaches  the  commanded 
position  faster  while  the  x-axis  link,  moving  slower,  is  the 
last  one  that  reaches  the  desired  position.  The  time 
required  for  the  movement  of  the  arm  to  be  completed  is  the 
time  required  by  the  slowest  (x-axis)  link  to  complete  its 
movement.  From  Figure  6.2  this  time  is  read  42  msec. 

The  error  curves  of  Figure  6.3  show  zero  steady 
state  error  for  all  links,  but  an  accurate  observation  of 
the  simulation  data  reveals  a  steady  state  accuracy  of  the 
order  of  10”5  inches. 

The  loaded  arm  under  different  load  conditions 
is  then  simulated  in  Figures  6.4  through  6.12.  In  these 
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Figure  6.7  Phase  Plane  Trajectory  for  Loaded  Arm 
(Load=0.9  -  No  Gravity) 
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Figure  6.9  Error  Curves  for  Loaded  Arm 
(Load=0 . 9  -  Gravity) 
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figures  the  effects  of  the  added  load  can  be  observed. 
Although  the  same  load  is  added  to  all  links,  the  link 
mostly  affected  by  this  load  is  the  y-axis  link  because  this 
is  the  lightest  one  and  its  relative  increment  of  the  weight 
(the  ratio  of  added  load  to  link  weight)  is  larger  than  in 
the  other  two  links.  Thus,  the  settling  time  for  the  y-axis 
link  is  increased  more  than  the  other  links  and  as  more  load 
is  added  to  the  arm,  the  time  difference  between  the 
settling  time  of  the  three  links  tends  to  zero. 

For  loads  up  to  0.83  oz/in/sec2,  the  critical 
load,  the  system  shows  good  curve  following  characteristics 
for  all  joints  as  depicted  in  Figures  6.4  -  6.6.  But  as  the 
load  exceeds  the  value  of  the  critical  load,  the  velocities 
of  the  three  links  are  reduced  and  they  can  not  track  the 
deceleration  curves.  Therefore,  when  they  reach  the 
commanded  positions,  their  velocities  do  not  reach  zero  but 
they  still  have  some  values  which  are  inversely  proportional 
to  the  weights  of  the  respective  links  as  shown  in  Figure 
6.10.  As  a  result  of  this  fact  the  three  links  overshoot  and 
undershoot  before  they  reach  steady-state,  as  shown  in 
Figure  6.11.  Consequently,  the  error  curves  oscillate  before 
they  reach  zero,  as  depicted  in  Figure  6.12. 

Then,  with  a  slight  modification  of  the  same 
simulation  program,  the  unloaded  arm  is  commanded  to  go  to 
the  desired  position,  take  a  load  and  return  to  the  initial 
location.  The  results  are  shown  in  Figures  6.13  -  6.15. 


TIME  t10's  SEC) 


Figure  6.13  shows  that  the  second-order  model 
and  servo  motor  have  good  curve  following  characteristics 
for  all  joints.  The  step  response  of  Figure  6.14  shows  that 
the  fastest  (y-axis)  link  reaches  the  desired  position  and 
waits  there  till  the  other  two  links  reach  this  position. 
When  the  slowest  (x-axis)  link  reaches  this  position,  the 
load  is  picked  up  by  the  robot  and  the  three  links  start  to 
move  to  their  initial  locations. 

b.  Disturbance  Rejection 

It  is  not  rare  that  disturbances,  although 
undesired,  are  present  in  the  robot  operation.  These 
disturbances  can  be  treated  as  random  impulse  inputs  applied 
at  any  time  during  the  robot  motion.  They  can  be  rejected 


from  a  well  designed  robot,  or  they  can  cause  failure  to  the 
desired  robot  motion  if  the  design  is  poor.  Next  the 
capability  of  the  system  to  reject  these  random  disturbances 
is  tested  using  the  DSL/VS  simulation  program  listed  in 


Appendix  D. 


An  impulse  of  2  msec  duration  is  selected  to 


represent  the  disturbance  and  it  is  applied  to  three  links 
in  different  times  during  their  motion.  First  the 
disturbance  is  applied  before  any  of  the  links  has  reached 
steady-state,  then  the  same  disturbance  is  applied  just 
before  the  first  (y-axis)  link  reaches  steady-state  and 
finally  after  all  links  have  reached  steady-state.  The 
results  of  the  three  cases  are  shown  in  Figures  6.16  -  6.21. 
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Figure  6.17  Step  Response  with  Disturbance 
Applied  at  T=10  msec  (No  Gravity) 
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Figure  6.21  Step  Response  with  Disturbance 
Applied  at  T=50  msec  (No  Gravity) 


♦.Lt, 


Although  the  same  disturbance  is  applied  to  all 


links,  the  link  affected  mostly  is  the  y-axis  link  which  is 


the  lightest  one.  Generally,  the  lighter  the  link  the  more 


sensitive  to  any  disturbance.  But  in  all  different  cases  the 


results  show  that  the  applied  disturbances  are  rejected  in  a 


time  period  which  is  also  inversely  proportional  to  link's 


weight.  Finally,  all  links  go  to  the  commanded  position. 


c.  Robustness 


The  robot  control  problem  is  to  design  a  stable 


and  robust  algorithm  to  enable  the  robot  to  follow  a 


specified  trajectory.  Therefore,  robustness  is  one  of  the 


main  considerations  in  robot  design.  Next  the  adaptive 


algorithm  is  tested  for  slight  (10%)  variations  of  the  servo 


motors  parametric  data  because,  although  the  same  servo 


motors  are  used  for  the  three  joints,  there  are  always  some 


tolerances  in  their  parametric  values  that  may  cause 


significant  changes  in  the  robot  behavior  if  the  system  is 


not  robust.  For  these  tests  the  simulation  program  of 


Appendix  C  is  used  again,  with  different  parametric  values 


for  each  run. 


The  effects  of  changing  the  torque  constant  (K^) 
and  the  back  emf  constant  (Kv)  for  the  unloaded  arm  by  10% 
are  depicted  in  Figures  6.22  -  6.25.  Figures  6.23  and  6.25 


show  a  very  small  variation  in  settling  time  which  is 


proportional  to  the  links  weights  and  has  the  same  sign  with 
the  variation  of  the  two  parameters.  Figures  6.22  and  6.24 
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Figure  6.25  Step  Response  for  Unloaded  Arm  with 
and  Kv  Decreased  by  10%  (No  Gravity) 
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show  very  good  curve  following  characteristics  either  for 
increase  or  decrease  of  the  above  two  parametric  values. 


Then  the  effects  of  a  movement  of  the  mechanical 


pole  towards  the  origin  are  examined.  For  the  pole  to  be 
moved  closer  to  the  origin,  the  resistance  (R)  is  decreased 
by  10%  and  the  inductance  (L)  is  increased  by  the  same 
percentage  amount.  Figures  6.26  and  6.27,  compared  with 
Figures  6.1  and  6.2,  show  no  change  in  the  behavior  of  the 
unloaded  arm.  If  load  is  placed  on  the  gripper  of  the  robot, 
as  Figures  6.28  -  6.31  show,  no  overshoot  occurs  for  loads 
up  to  0.94  02/ in/sec2.  Comparison  with  Figures  6.4  -  6.8 
reveals  that  the  critical  load  is  increased  from  0.83  to 
0.94  oz/ in/sec2. 

With  the  mechanical  pole  moved  towards  the 
origin,  if  the  constants  and  Kv  are  increased  by  10% 
Figures  6.32  -  35  show  that  the  robot  can  carry  load  up  to 
1.08  oz/in/sec2  without  overshooting.  In  the  opposite  case, 
if  and  Kv  are  decreased  by  the  same  amount,  Figures  6.36 
-  39  show  that  load  up  to  0.80  oz/in/sec2  can  be  carried 
without  overshooting. 


The  adaptive  system  is  tested  again  for  different 
load  conditions,  disturbance  rejection  and  robustness  in  the 
case  of  motor  parameters  variation,  but  this  time 
gravitational  torques  are  included  in  the  simulation. 
Actually,  the  only  link  which  senses  gravitational  torques 
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Figure  6.31  Step  Response  for  Loaded  Arm  with 
R  Decreased  and  L  Increased  by  10% 
(Load=0.98  -  No  Gravity) 
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Figure  6.33  Step  Response  for  Loaded  Arm  with 
R  Decreased  and  Kt,  Kv,  L  Increased  by  10% 
(Load=1.08  -  No  Gravity) 
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(Load=1.12  -  No  Gravity) 
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Figure  6.38  Phase  Plane  Trajectory  for  Loaded  Arm 
K{.,  Kv,  R  Decreased  and  L  Increased  by  10% 
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is  the  vertical  one  because  the  other  two  links  moving  in  a 
direction  perpendicular  to  the  direction  of  gravity  are  not 
affected  at  all. 

The  adaptive  system  is  tested  first  without  carrying 
any  load,  using  the  DSL/VS  simulation  program  listed  in 
Appendix  E.  Figures  6.40,  6.41  and  6.42  seem  to  be  identical 
to  Figures  6.1,  6.2  and  6.3  which  are  the  respective  ones 
for  the  robot  operating  in  a  no  gravitational  environment, 
and  especially  the  step  response  and  the  error  curves  show 
that  the  three  model  and  servo  motors  track  together  to  the 
commanded  position  with  zero  steady  state  error.  An  accurate 
observation  of  the  simulation  data  reveals  a  positive  steady 
state  error  of  the  order  of  10“5  inches  for  the  z-axis  link 
of  the  robot  arm  operating  under  gravitational  torques.  This 
means  that  the  tip  of  the  z-axis  link  never  reaches  the 
commanded  position  but  stops  10-5  inches  lower  than  the 
desired  position.  The  error  curves  of  the  other  two  links  in 
steady  state  condition  oscillate  with  an  amplitude  of  the 
order  of  10“5  inches,  as  they  do  in  the  no  gravity  case. 
Therefore,  the  only  difference  observed  in  the  behavior  of 
the  unloaded  robot  arm  operating  under  gravitational  torques 
is  the  steady  state  error  of  the  vertical  (z-axis)  link. 

Then  the  robot  arm  is  tested  under  the  same  loaded 
conditions  applied  in  the  no  gravity  case.  The  simulation 
results  show  that  the  behavior  of  the  robot  arm  under 
gravitational  torques  is  almost  identical  to  the  behavior  of 
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the  robot  arm  operating  in  a  gravity  free  environment.  The 
only  difference  observed  is  a  small  (3%)  change  in  the 
capability  of  the  tested  arm  to  carry  loads  without 
overshooting  depending  on  the  direction  of  the  move.  If  the 
arm  is  moving  against  gravity,  the  change  is  positive 
(Figures  6.43,  6.44  and  6.45),  but  if  it  is  moving  with 
gravity  the  change  is  negative. 

Since  the  simulation  results  are  very  much  the  same 
with  the  results  of  the  robot  arm  under  no  gravity,  most  of 
them  are  omitted  in  this  section  and  just  a  few  indicative 
figures  are  included  to  illustrate  the  nearly  identical 
behavior  of  the  three  link  rectangular  robot  operating  under 
gravitational  and  no  gravitational  torques. 

After  that,  the  robot  arm  is  tested  for  disturbance 
rejection  applying  the  same  disturbances  mentioned  in  part 
l.b  of  this  chapter.  Figures  6.46  and  6.47  are  the  results 
obtained  using  the  simulation  program  listed  in  Appendix  F, 
and  show  that  the  applied  disturbances  are  rejected  and  all 
links  go  to  the  commanded  position. 

Finally,  the  robot  arm  is  tested  for  slight  (10%) 
variation  of  motor  parameters.  Simulation  results  show  no 
noteworthy  change  in  the  characteristics  of  the  system  which 
behaves  exactly  as  does  under  no  gravity  as  depicted  in 
Figures  6.48  and  6.49. 
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Figure  6.48  Phase  Plane  Trajectory  for  Loaded  Arm  with 
K{-,  Kv,  R  Decreased  and  L  Increased  by  10% 
(Load=0.82  -  With  Gravity) 
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A.  INTRODUCTION 


In  this  chapter  the  mathematical  model  of  the  three  link 
revolute  robot  is  developed,  making  use  of  the  Lagrangian 
mechanics  as  this  method  allows  us  to  obtain  the  dynamic 
equations  for  very  complicated  systems  in  a  rather  simple 
manner.  The  Lagrangian  formulation  describes  the  behavior  of 
a  dynamic  system  in  terms  of  work  and  energy  stored  in  the 
system  rather  than  forces  and  momentums  of  the  individual 
members  involved . 

The  dynamic  equations,  which  relate  forces  and  torques 
to  positions,  velocities,  and  accelerations,  are  first 
obtained  and  then  solved  in  order  to  obtain  the  equations  of 
motion  of  the  robot.  Given  forces  and  torques  as  input  these 
equations  specify  the  resulting  motion  of  the  robot.  This 
approach  is  computationally  difficult  but  gives  results  that 
provide  insight  into  both  the  linear  and  nonlinear  operation 
of  robot  arms. 

Using  the  Lagrangian  method  makes  possible  the 
computation  of  the  interaction  of  gravitational,  loading, 
and  centripetal  torques  and  forces  dependent  on  the  external 
forces,  the  inertias  of  the  various  components  of  the  robot 
arm,  and  the  structure  of  the  system.  Coriolis  acceleration 
is  derived  as  part  of  the  overall  analysis. 


B.  MODEL  DEVELOPMENT 

The  three  link  robot,  chosen  to  be  tested,  is  depicted 
in  Figure  7.1.  There  are  three  rotary  joints  connecting  the 
three  links.  A  gripper  is  mounted  at  the  end  of  LINK3  which 
can  carry  a  load.  It  is  assumed  that  the  three  links  are 
massless  and  the  equivalent  masses,  mj_,  m2,  m3 ,  are  lumped 
at  the  end  of  the  respective  links.  The  lengths  of  the  three 
links  are  denoted  dlf  d2,  d3  respectively,  and  T1#  T2,  T3 
represent  the  external  driving  torques  to  the  joints. 

Direct-drive  motors  are  used  to  drive  the  three  joints. 
"Direct-drive”  refers  to  the  fact  that  the  links  of  the 
robot  are  coupled  directly  to  the  respective  motor  shafts 
without  going  through  any  gearing.  The  advantage  of  this 
scheme  is  that  one  immediately  eliminates  the  effects  of 
backlash,  joint  flexibility,  etc.,  which  arise  from  the  use 
of  gears.  However,  there  are  two  significant  negative  co¬ 
sequences.  First,  the  presence  of  large  gear  ratios 
effectively  decouples  the  dynamic  of  each  motor  from  the 
rest,  which  in  turn  simplifies  the  control  problem  [Ref. 9]. 
When  direct  drive  is  used,  this  isolation  effect  is  no 
longer  present,  and  one  is  forced  to  deal  with  more 
complicated  equations.  The  second  effect  concerns  the 
actuator  dynamics.  In  a  robot  with  gears,  even  a  small 
motion  of  a  link  results  in  several  revolutions  of  the 
corresponding  motor  shaft,  so  that  the  standard  method  of 
modeling  a  motor  as  an  inertia  and  a  viscous  damper  is  quite 


C.  EQUATIONS  OP  MOTION 

The  Lagrangian  L  is  defined  as  the  difference  between 
the  kinetic  energy  K  and  the  potential  energy  V  of  the 
system. 

L  *  K  -  V  (7.1) 

Generally,  the  dynamic  equations,  in  terms  of  the 
coordinates  used  to  express  the  kinetic  and  potential 
energy,  are  obtained  as 

d  / 0L  \  3l 

- — —) - '  i=l,  .  . . , n  (7.2) 

dt  \cm/  3<Ii 

where 

*  the  coordinates  in  which  the  kinetic  and  potential 
energy  are  expressed 

q^  *  the  corresponding  velocity 

*  the  corresponding  force  or  torque,  depending  upon 
whether  q^  is  a  linear  or  an  angular  coordinate. 

n  =  the  number  of  links  (degrees  of  freedom) 

These  coordinates,  forces,  and  torques  are  referred  to  as 
generalized  coordinates,  forces,  and  torques. 

In  order  to  apply  equation  7.2  to  the  three  link 
revolute  robot,  the  rotation  angles  e2,  and  e3  are 

chosen  as  the  generalized  coordinates.  Therefore,  F^,  F2, 

and  F3  represent  the  generalized  torques  about  the  axes  of 
rotation  of  the  three  links  and  from  now  on  are  renamed  as 
Tl,  t2'  and  t3*  Solving  equation  7.2  for  the  three  links. 


the  following  system  of  nonlinear,  second-order  differential 
equations  has  been  derived  and  use  as  the  basic  mathematical 
model  for  the  simulation  of  the  three  link  revolute  robot. 
The  detailed  derivation  is  presented  in  Appendix  G. 

(Dn+Jmi)©!  =  T1+DH20102+D1130103  (7*3) 

(D22+Jm2)G2  =  t2"d23®3+d223®2®3+d233®32_d211®12_G2  (7*4) 
(D33+Jm3)©3  =  T3-D32e2-D311012-D322e22-G3  (7-5) 

where  _ 

Du  =  m3  (d2cose2+d3cos  (02+e3)  )  2+m2d22cos2e2 

d22  =  (m2+m3)d22+2in3ci2d3cos03+m3d32 

d23  =  m3d2d3cos03+in3d32 

D32  =  m3d2d3Cos03+m3d32 

D33  =  m3d32 

dH2  =  2  [  (m2+m3 )  d22sine2cos02+m3d2d3sin  (292+03 ) 
+m3d32sin(02+03) cos (02+03) ] 
d113  =  2[m3d2d3sin(02+03)cos02 

+m3d32sin(02+03)cos(02+03) ] 
d211  =  (m2+m3)d22sin02cos02+m3d2d3sin(202+03) 
+m3d32sin(02+03) cos (02+03) 
d223  =  2m3d2d3sin03 


D233  =  m3d2d3sin03 

D311  =  m3d2d3sin (02+03) cos02 

+m3d32sin(02+03) cos (02+03) 
d322  =  m3d2d3sin03 

g2  =  (®2‘t‘m3)<3<i2cos02",‘in3,3d3cos(02+03) 


=  m3gd3COS(e2+©3) 

=  0.033  =  motor  inertia  for  JOINT  1 

=  0.033  =  motor  inertia  for  JOINT  2 

=  0.033  =  motor  inertia  for  JOINT  3 

=  15  inches 
=  10  inches 


=  10  inches 


mi  =  0.268  oz/in/sec2 
-T&2  ~  0*-227  oz/in/sec2 
m3  =  0.041  oz/in/sec2 
g  =  386.4  in/sec2 

Equations  7.3,  7.4,  and  7.5  can  be  written  in  the  form 


T1  ~  (Dn+Jmi)  ei 

~  d112®1©2  “  d113®1®3 

t2  =  (D22+Jm2)®2  +  d23®3 

+  D211®12  "  d233®32 
•  • 

“  D223e203 


T3  “  (D33+Jm3)e3  +c,32e2 
+  d311®12  +  °322®2‘ 


(7.6) 


(7.7) 


(7.8) 


In  these  equations,  the  coefficients  of  the  form 

Dii+Jmi  (Dll+Jml >  D22+Jm2'  D33+Jm3)  are  known  as  the 

effective  inertia  at  joints  i.  Acceleration  at  joint  i 


I 
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causes  torque  known  as  as  inertial  torque.  The  coefficients 
of  the  form  D^j  (D23,  D32)  are  known  as  the  coupling  inertia 
between  joints  i  and  j,  as  an  acceleration  at  joint  i  or  j 
causes  a  torque  at  joint  j  or  i,  respectively.  The  terms  of 

the  form  Dijj®j2  (°211®12'  °233®32'  °311®12'  °322®22) 
represent  the  centripetal  forces  acting  at  joint  i  due  to  an 

angular  velocity  at  joint  j.  The  terms  of  the  form  DiijejGj 
«  •  «  «  •  « 

(D112®1®2'  D113®1®3'  d223®2®3)  represent  the  Coriolis  force 
acting  at  joint  i  due  to  angular  velocities  at  both  joints  i 
and  j .  The  terms  of  the  form  represent  the  gravitational 
forces  acting  at  joint  i.  [Ref. 10] 
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VIII.  THE  ADAPTIVE  MODEL  FOR  THE  THREE  LINK  REVOLUTE  ROBOT 

A.  INTRODUCTION 

The  same  second  order  model  developed  in  Chapter  III  and 
used  in  the  rectangular  robot,  will  be  operated  again,  open 
loop,  to  drive  the  servo  motors  of  the  joints  of  the 
revolute  robot  and  control  its  output,  C.  For  the  reasons 
described  in  chapter  IV  the  model  states  and  gain  constant 
must  be  adapted  in  such  a  way  that  the  ideal  motors  imitate 
the  real  ones.  Since  the  adaptive  model  of  Figure  4.8,  used 
for  the  rectangular  robot,  is  used  for  the  revolute  robot 
too,  the  adaptive  algorithm  to  update  the  model  states  and 
gain  constant  is  identical  with  the  one  obtained  in  Chapter 
IV  and  its  derivation  is  not  repeated. 

In  this  chapter,  the  selection  of  the  servo  motors  is 
first  discussed  and  then  the  initial  values  of  the  model 
gain  constants  (Km)  are  obtained. 


B.  SELECTION  OF  POSITIONING  SERVO  MOTORS 

The  same  permanent  magnet  servo  motor  used  for  the 
rectangular  robot  is  also  used  for  the  revolute  robot  and 
its  parametric  data  are  listed  in  Table  1  [Ref. 6].  The  links 
of  the  robot  are  coupled  directly  to  the  respective  motor 
shafts  without  going  through  any  gearing  ("direct-drive" 
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motors)  .  The  same  motor  drive  is  used  for  the  three  joints 
with  the  same  power  supply. 


C.  CALCULATION  OF  GAIN  CONSTANTS  (Km) 

To  calculate  the  transfer  functions  of  the  servo  motors, 
the  effective  joint  inertias  must  be  first  obtained  as 
following 


JEF1  =  Dll+Jml 
JEF2  =  D22+Jm2 
JEF3  =  °33+Jm3 


(8.1) 

(8.2) 

(8.3) 


where  D^,  D22  and  D33  are  given  in  Chapter  VII, 

Substitution  of  the  parameters,  for  02=0 *  and  ©3=0°  gives 

JEF1  “  39.133  oz.in.sec2 
JEF2  ~  39.133  oz.in.sec2 
Jrr-!  =  4.133  oz.in.sec2 


Plugging  these  results  and  parameters  from  Table  1  into 
equation  3.1,  the  transfer  functions  of  the  three  servo 
motors  are  obtained  as 


G^(s)  - 


G2(s)  - 


G3 (s)  - 


9.88 


s(s/0. 041+1) (s/9100+1) 


9.88 


s(s/0. 041+1) (s/9100+1) 


9.88 


s (s/0 . 39+1) (s/9100+1) 


rad/ volt 


rad/volt 


rad/volt 


(8.4) 


(8.5) 


(8.6) 


Using  these  results  the  gain  constants  of  the  model 
motors  (Km)  were  determined.  The  Open  Loop  Bode  plots  of  the 
three  servo  motors  are  depicted  in  Figures  8.1,  8.3  and  8.5, 
respectively.  These  plots  show  that  the  -40  db/decade  slope 
intersects  the  0  db  axis  at6Ji=0.65  rad/sec  for  the  JOINT1 
motor,  GJ2=0* 65  rad/sec  for  the  J0INT2  motor  andCJ3=2  rad/sec 
for  the  JOINT3  motor.  Making  use  of  the  above  frequencies  as 
gain  crossover  frequencies  for  the  second  order  ideal 
motors,  we  obtain 


Kml  =  0.652  =  0.4225  rad/volt 
Km2  =  °*6s2  -  0.4225  rad/volt 
Km3  =  2 2  =  4  rad/volt 


The  Open  Loop  Bode  plots  of  the  Km/82  ideal  motors  are 
depicted  in  Figures  8.2,  8.4  and  8.6. 
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Figure  8.1  Open  Loop  Bode  Plot  of  the  JOINT  Servo  Motor 
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A.  PLANNING  THE  SIMULATION  STUDIES 

In  this  chapter,  the  model  developed  in  Chapter  VII  is 
tested  for  the  Voltage  Source  Drive.  First  the  model  is 
tested  in  a  gravity- free  environment  and  then  the  same  tests 
are  repeated  with  gravitational  torques  taken  into  account. 

The  tested  arm  is  a  point-to-point  arm  and  for  this 
thesis  a  free  work  space  is  assumed.  A  unit  step  input 
command  is  applied  in  all  joints  at  the  same  time  to  assure 
realistic  interaction  between  the  three  links.  For  each 
simulation  run  the  phase  plane  plot,  the  step  response  and 
the  error  curves  are  obtained  in  order  to  study  the  behavior 
of  the  system. 


B.  SIMULATION  STUDIES  OF  THE  ADAPTIVE  SYSTEM 


1.  Gravitv-free  Environment 

The  three  link  revolute  robot  in  a  gravity-free 
environment  is  tested  first  under  different  load  conditions, 
then  for  disturbance  rejection  and  finally  the  robustness  of 
the  system  is  tested  for  a  slight  variation  of  the 
parametric  data  of  the  servo  motors. 


a.  Different  Load  Conditions 


The  adaptive  model  is  tested  first  without 
carrying  any  load  and  then  under  different  load  conditions, 


using  the  DSL/VS  simulation  program  listed  in  Appendix  H. 
Figures  9.1,  9.2  and  9.3  illustrate  the  phase  plane  plot, 
the  step  response  and  the  error  curves,  respectively,  for 
the  unloaded  arm. 

Figure  9.1  shows  that  the  model  and  servo  motors 
have  good  curve  following  characteristics  for  all  joints. 
Observing  the  phase  plane  plots  for  J0INT1  and  J0INT2,  one 
would  expect  J0INT2  to  reach  the  commanded  position  faster 
than  JOINT1 .  Contradictorily,  the  step  response  of  Figure 
9.2  shows  that  J0INT1  reaches  steady  state  a  little  earlier 
than  J0INT2 .  The  explanation  is  that  the  upward  motion  of 
the  LINK3  creates  an  interaction  torque  on  J0INT2  servo 
motor  and  it  takes  time  (about  40  msec)  for  the  JOINT2  to 
overcome  the  reaction  torque  and  start  accelerating.  The 
time  required  for  the  movement  of  the  arm  to  be  completed  is 
the  time  required  by  the  slowest  link  (LINK2)  to  complete 
its  movement.  From  Figure  9.2  this  time  is  read  335  msec. 
The  error  curves  of  Figure  9.3  show  zero  steady  state  error 
for  the  three  links,  but  a  more  accurate  observation  of  the 
simulation  data  reveals  a  steady  state  accuracy  of  the  order 
of  10"5  inches. 

The  simulation  results  of  the  loaded  arm  under 
different  load  conditions  are  depicted  in  Figures  9.4- 
9.15.  In  these  figures  the  effects  of  the  added  load  can  be 
observed.  The  phase  plane  plot  of  Figure  9.4  shows  that  at 
the  beginning  of  the  motion,  J0INT2  servo  motor  develops  a 


Figure  9.1  Phase  Plane  Trajectory  for  Unloaded  Arm 

(No  Gravity) 


STEP  RESPONSE 


small  negative  velocity  (in  the  opposite  direction  of  the 
desired  move) .  This  negative  velocity  is  caused  by  the 
torque  created  on  the  J0INT2  due  to  coupling  inertia  between 
J0INT2  and  J0INT3.  This  torque  is  proportional  to  the 
acceleration  of  J0INT3  which  is  large  at  the  beginning  of 
the  move.  As  the  acceleration  of  J0INT3  starts  decreasing, 
J0INT2  servo  motor  overcomes  this  torque  and  starts  moving 
in  the  direction  of  the  commanded  move.  When  it  reaches  the 
commanded  curve  it  can  not  follow  the  curve  due  to  the 
interactive  torque  of  the  still  accelerating  J0INT3  servo 
motor.  As  soon  as  J0INT3  reaches  the  commanded  position,  it 
starts  decelerating  and  the  J0INT2  servo  motor  catches  the 
commanded  velocity  curve  and  follows  it  down  to  the 
commanded  position. 

The  step  response  and  the  error  curves  of  Figure 
9.5  and  9.6  show  the  initial  movement  of  JOINT2  in  the 
opposite  direction.  The  other  two  joints  move  directly  to 
the  commanded  position.  The  joint  mostly  affected  by  the 
load  is  J0INT2  because  the  added  load  increases  the  coupling 
inertia  between  J0INT2  and  J0INT3  and  consequently  increases 
the  torque  applied  on  J0INT2 . 

If  the  added  load  increases  Figure  9.7  shows 
that  the  negative  velocity  of  J0INT2  at  the  beginning  of  the 
motion  becomes  greater  and  the  settling  time  for  J0INT2  and 
J0INT3  increase,  as  depicted  in  Figure  9.8  and  9.9.  But  as 
more  load  is  added,  the  Coriolis  force  acting  on  J0INT1,  due 
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Figure  9.9  Error  Curves  for  Loaded  Arm 
(Load=0.1  -  No  Gravity) 
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to  the  angular  velocities  at  all  joints,  increases  and  the 
increased  torque  at  J0INT1  is  the  reason  that  the  J0INT1 
servo  motor  can  not  follow  the  commanded  velocity  curve  as 
soon  as  it  reaches  the  curve.  But  as  J0INT3  starts 
decelerating,  the  torque  at  J0INT1  decreases  and  finally 
JOINT1  catches  and  follows  the  commanded  curve  to  the 
desired  position.  Because  of  the  increased  angular  velocity 
of  J0INT1  its  settling  time  is  reduced. 

Figure  9.10  show  that  the  critical  load  for  this 
robot  configuration  is  0.157  oz/in/sec2  because  for  this 
value  of  load,  JOINT1  catches  the  commanded  curve  just  at 
the  commanded  position.  Also  it  can  be  observed  from  the 
same  figure  that  while  the  initial  negative  velocity  of 
JOINT2  is  decreased,  J0INT2  is  capable  to  follow  the 
commanded  curve  as  soon  as  it  reaches  it.  Figures  9.11  and 
9.12  are  the  step  response  and  the  error  curves, 
respectively,  for  this  critical  load. 

For  any  load  greater  than  the  critical  one, 
JOINT1  catches  the  commanded  curve  after  it  reaches  the 
commanded  position.  So,  when  J0INT1  reaches  the  commanded 
position  its  velocity  is  not  zero  but  still  has  a  value 
which  is  proportional  to  the  load.  As  a  result  of  this  fact, 
JOINT1  overshoots  before  it  reaches  steady  state  condition. 
As  the  added  load  is  further  increased  (load=0.2 
oz/in/sec2),  JOINT2  starts  overshooting  too,  as  illustrated 
in  Figures  9.13  -  9.15. 
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Figure  9.15  Error  Curves  for  Loaded  Arm 
(Load=0 . 2  -  No  Gravity) 
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From  the  study  of  the  above  results  for  the 
three  link  revolute  robot  operating  under  different  load 
conditions,  it  becomes  obvious  that  the  greater  the  added 
load  the  greater  the  initial  negative  velocity  of  J0INT2  and 
the  greater  overshooting  for  the  J0INT1  and  JOINT2  servo 
motors . 


b.  Disturbance  Rejection 

To  test  the  capability  of  the  three  link 
revolute  robot  to  reject  random  disturbances,  an  impulse 
with  an  amplitude  of  50  rad  and  a  duration  of  10  msec  is 
applied  to  three  joints  in  different  times  during  their 
motion.  Making  use  of  the  DSL/VS  program  listed  in  Appendix 
I,  first  the  disturbance  is  applied  just  before  J0INT3 
reaches  steady  state,  then  the  same  disturbance  is  applied 
just  before  JOINT1  and  JOINT2  reach  steady  state  and  finally 
after  all  joints  have  reached  steady  state. 

In  the  first  case  Figure  9.16  shows  that  the 
applied  disturbance  causes  JOINT3  to  be  unable  to  catch  the 
curve  before  it  reaches  the  commanded  position  and  therefore 
JOINT3  overshoots  as  illustrated  in  Figure  9.17.  In  the 
second  case  Figure  9.18  shows  that  due  to  the  applied 
disturbance  J0INT1  and  J0INT2  catch  the  curve  just  after 
they  pass  the  commanded  position  and  the  resulting  small 
overshooting  of  these  joints  is  shown  in  Figure  9.19.  In  the 
third  case,  Figure  9.20  shows  that  the  robot  is  disturbed 
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9.18  Phase  Plane  Trajectory  with  Disturbance 
Applied  at  T=270  msec  (No  Gravity) 
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Figure  9.19  Step  Response  with  Disturbance 
Applied  at  T=270  msec  (No  Gravity) 
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after  all  joints  have  reach  steady  state  condition  and  the 
overshooting  of  all  joints  can  be  observed  in  Figure  9.21. 

Although  the  same  disturbance  is  applied  to  all 
joints,  the  joint  mostly  affected  is  J0INT3  which  is  the 
lightest  one.  Finally  in  all  different  cases  the  simulation 
results  show  that  the  applied  disturbances  are  rejected  and 
all  joints  return  to  the  commanded  position.  Therefore,  the 
tested  robot  is  capable  to  reject  any  random  disturbance, 
c.  Robustness 

Next  the  adaptive  algorithm  is  tested  for  slight 
(10%)  variations  of  the  servo  motors  parametric  data  in 
order  to  check  if  the  designed  system  is  robust.  For  these 
tests  the  simulation  program  listed  in  Appendix  H  is  used 
again,  changing  some  of  the  parametric  values  for  each 
different  run. 

First  the  torque  constant  (Kt)  and  the  back  emf 
constant  (Kv)  are  changed  10%  and  the  results  are  shown  in 
Figures  9.22  -  9.25.  The  phase  plane  plots  of  Figures  9.22 
and  9.24  show  very  good  curve  following  characteristics  for 
positive  or  negative  change  of  the  above  constants.  The  step 
response  of  Figures  9.23  and  9.24  are  nearly  identical  and 
only  an  accurate  observation  of  the  simulation  data  reveals 
a  small  variation  in  the  settling  time  which  has  the  same 
sign  with  the  variation  of  the  two  constants. 
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Then  the  mechanical  pole  of  the  servo  motor  is 
moved  toward  the  origin  and  the  effects  of  this  change  in 
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the  behavior  of  the  system  are  observed.  Maximum  movement 
of  the  pole  is  achieved  when  the  resistance  (R)  is  decreased 
and  the  inductance  (L)  is  increased  with  the  same  amount 
(10%),  simultaneously.  In  comparison  with  Figures  9.1  and 
9.2,  Figures  9.26  and  9.27  show  that  this  movement  does  not 
affect  at  all  the  behavior  of  the  unloaded  arm.  But  Figures 
9.28  -  9.31  show  that  the  loaded  arm  does  not  overshoot  for 
loads  up  to  0.185  oz/in/sec2.  Therefore,  as  result  of  this 
movement  of  the  mechanical  pole  of  the  servo  motor,  the  load 
for  which  overshooting  starts  is  increased  from  0.157  to 
0.185  oz/ in/sec2,  which  corresponds  to  18%  increase  of  the 
critical  load. 

After  the  mechanical  pole  has  been  moved  toward 
the  origin,  the  constants  and  Kv  are  changed  again  and 
the  effects  of  this  combination  of  changes  are  observed. 
Figures  9.32  -  9.35  show  that  for  10%  increased  values  of 
and  Kv  the  critical  load  is  further  more  increased  to  the 
value  of  0.215  oz/in/sec2,  while  for  decreased  values  of  the 
above  constants  by  the  same  amount.  Figures  9.36  -  9.39  show 
that  the  critical  load  is  reduced  to  0.160  oz/in/sec2. 

From  the  results  of  the  above  tests,  it  becomes 
clear  that  any  small  change  in  the  parameters  of  the  servo 
motors,  which  is  possible  in  practice,  does  not  affect 
dramatically  the  behavior  of  the  system.  Therefore,  the 
designed  robot  arm  is  robust. 


Figure  9.27  Step  Response  for  Unloaded  Arm  with  f. 

R  Decreased  and  L  Increased  by  10%  (No  Gravity)  * 
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Figure  9.29  Step  Response  for  Loaded  Arm  with 
R  Decreased  and  L  Increased  by  10% 
(Load=0.18  -  No  Gravity) 
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Figure  9.36  Phase  Plane  Trajectory  for  Loaded  Arm  with 
K-j-,  Kv,  R  Decreased  and  L  Increased  by  10% 
(Load=0.155  -  No  Gravity) 
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The  same  tests  are  applied  to  the  adaptive  model 
operating  under  gravitational  torques.  First  a  unit  step 
is  used  as  input  in  all  joints  and  then  some  combinations  of 
positive  and  negative  unit  step  input  commands  are  used  for 
JOINT2  and  J0INT3 ,  to  test  the  system  for  the  existence  of 
excess  interactive  torques. 

a.  Different  Load  Conditions 


The  phase  plane  plot  of  the  unloaded  arm,  which 
is  depicted  in  Figure  9.40,  show  that  at  the  beginning  of 
the  movement  JOINT2  servo  motor  develops  negative  velocity, 
which  has  larger  magnitude  than  the  respective  one  when  the 
arm  operates  in  a  no  gravitational  environment  because  the 
gravitational  torque  is  added  to  the  torque  developed  on 
JOINT2  due  to  coupling  inertia  between  J0INT2  and  JOINT3 .  As 
the  acceleration  of  JOINT3  starts  decreasing,  JOINT2  servo 
motor  overcomes  the  disturbance  torque  and  starts  moving  in 
the  commanded  direction.  When  it  reaches  the  curve,  it 
follows  the  curve  down  to  the  commanded  position.  The  other 
two  joints  show  very  good  curve  following  characteristics. 

The  step  response  and  error  curves  of  Figures 
9.41  and  9.42  show  clearly  the  motion  of  J0INT2  in  the 
opposite  direction,  initially.  Figure  9.41  shows  also  that 
it  takes  about  100  msec  for  J0INT2  to  overcome  the  reaction 
torque.  The  time  required  for  the  movement  to  be  completed 
is  read  as  420  msec,  which  is  25%  increased  comparing  to  the 
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Figure  9.41  Step  Response  for  Unloaded  Arm 
(With  Gravity) 


130 


Cl  IRAQI  C2  IRAQI  C3  IRAQI 


respective  time  for  the  no  gravity  case.  The  simulation  data 
reveal  a  steady  state  error  of  the  order  of  10-5  inches. 

In  the  case  of  a  loaded  arm,  a  larger  magnitude 
of  negative  velocity  is  observed  in  the  phase  plane  plots  of 
Figures  9.43  and  9.46,  because  of  the  greater  gravitational 
torques.  The  other  two  joints  continue  to  show  very  good 
curve  following  characteristics  and  all  joints  are  capable 
to  follow  the  commanded  curve  immediately  after  they  catch 
it.  The  maximum  load  that  the  designed  robot  arm  can  carry 
was  found  to  be  0.195  oz/in/sec2.  From  the  step  responses  of 
Figures  9.44  and  9.47  it  can  be  observed  that  the  developed 
reaction  torque  at  the  beginning  of  the  movement  results  in 
J0INT2  servo  motor  moving  initially  in  the  opposite 
direction.  The  joint  mostly  affected  by  the  load  is  JOINT2 
and  the  settling  time  for  this  joint  increases  proportional 
to  the  added  load.  The  error  curves  of  Figure  9.45  and  9.48 
show  that  the  error  of  JOINT2  at  the  beginning  of  the 
movement  is  also  proportional  to  the  carried  load  and  the 
effects  of  the  added  load  in  the  settling  time  of  JOINT2 
servo  motor  can  be  observed  clearly. 

In  some  applications  the  initially  developed 
negative  velocity  of  J0INT2  may  cause  problems.  To  avoid 
this  undesired  situation,  a  step  input  command  delayed  by 
150  msec  is  applied  to  JOINT3  servo  motor.  Figure  9.49  shows 
that  the  initially  developed  negative  velocity  of  JOINT2 
servo  motor  is  eliminated  and  a  zig  -  zag  shape  in  its  phase 
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plane  plot  is  observed  due  to  reaction  torque  produced  by 
this  servo  motor.  The  step  response  of  Figure  9.50  shows 
that  the  settling  time  for  J0INT2  servo  motor  remains  the 
same  while  the  movement  of  J0INT2  in  the  opposite  direction 
is  eliminated. 

Then  some  combinations  of  positive  and  negative 
unit  step  input  commands  are  applied  to  J0INT2  and  JOINT3 
servo  motors  to  simulate  the  movements  of  LINK2  and  LINK3  in 
the  direction  of  gravity.  Figures  9.51  -  9.54  show  that  when 
both  LINK2  and  LINK3  move  in  the  direction  of  gravity  (move 
#2),  JOINT3  overshoots  for  a  relatively  small  load  (0.06 
oz/in/sec2).  The  phase  plane  plots  of  these  figures  show 
that  for  a  considerable  part  of  their  movement,  JOINT1  and 
JOINT2  move  with  velocity  greater  than  the  commanded  one.  In 
the  case  that  LINK2  moves  in  the  direction  of  gravity  and 
LINK3  moves  against  gravity  {move  #3),  Figures  9.55  -  9.58 
show  that  J0INT2  overshoots  for  a  slightly  larger  load  but 
JOINT1  and  J0INT3  follows  accurately  the  commanded  curve. 

The  DSL/VS  simulation  program  used  to  test  the 
three  link  revolute  robot  operating  in  a  gravitational 
environment  under  different  load  conditions  is  listed  in 
Appendix  J. 

b.  Disturbance  Rejection 


Making 

use 

of 

the 

DSL/ VS 

simulation  program 

listed  in 

Appendix 

K, 

the 

same 

disturbances  mentioned  in 

part  l.b 

of  this 

chapter 

are 

applied 

to  the  robot  arm 
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Figure  9.51  Phase  Plane  Trajectory  for  Move  #2 
(Load=0.055  -  With  Gravity) 
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Figure  9.57  Phase  Plane  Trajectory  for  Move  #3 
(Load=0.07  -  With  Gravity) 
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operating  under  gravitational  torques,  simulation  results 
illustrate  the  nearly  identical  behavior  of  the  robot  arm 
operating  either  under  gravitational  torques  or  in  a  gravity 
free  environment.  Figures  9.59  and  9.60  show  that  the 
applied  disturbances  are  rejected  and  all  joints  reach  the 
desired  position. 

c.  Robustness 

Finally,  the  revolute  robot  is  tested  again  for 
slight  (10%)  variation  of  the  servo  motors  parameters. 
Simulation  results  do  not  reveal  any  noteworthy  change  in 
the  behavior  of  the  system  which  is  almost  identical  to  the 
behavior  of  the  system  in  the  no  gravity  case,  as  depicted 


Figures  9.60  Step  Response  with  Disturbance 
Applied  at  T=90  msec  (With  Gravity) 
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Figure  9.62  Step  Response  for  Loaded  Arm  with 
Kt'  Kv>  R  Decreased  and  L  Increased  by  10% 
(Load=0.155  -  With  Gravity) 
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As  a  result  of  the  research  conducted  In  this  thesis, 
the  control  of  both  the  three  link  rectangular  and  revolute 
robots,  using  the  curve  following  technique  with  an  adaptive 
simulation  model  appears  feasible. 

In  this  study,  direct-drive  servo  motors  were  used.  The 
use  of  these  motors  has  several  important,  advantages 
including  no  backlash,  small  friction  and  high  mechanical 
stiffness,  but  the  complicated  dynamics  resulting  from 
inertia,  interactions  and  nonlinearites  is  also  more 
prominent  than  that  of  a  robot  with  gears  because  in  this 
case  the  disturbance  torques  are  reduced  proportionally  to 
the  gear  ratio. 

Both  configurations  were  tested  for  different  load 
conditions,  for  random  disturbance  rejection  and  for 
robustness  in  the  case  of  servo  motor  parameter  variations. 
Initially,  a  gravity-free  environment  was  assumed  and  then 
the  same  tests  were  repeated  with  gravitational  torques 
included.  In  all  cases  actuator  dynamics,  coupling  inertia, 
centripetal  and  coriolis  forces  were  included. 

From  simulation  results  it  was  observed  that  as  the 
added  load  was  increased  the  robot  arms  started  overshooting 
which  may  cause  problems  in  some  applications.  The 
capability  of  the  arms  to  carry  larger  loads  may  be  improved 
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if  the  curve  scaling  constant,  ,  is  designed  to  be 
adaptive.  Further  thesis  research  in  this  case  is  required. 
The  randomly  applied  disturbances  were  rejected  in  all  cases 
and  the  arms  were  driven  to  the  commanded  position.  The 
slight  variation  of  the  servo  motor  parameters  did  not  show 
any  noteworthy  change  in  the  behavior  of  both  robot  arms. 

A  delayed  step  input  command  was  used  to  prevent 
undesired  motion  of  the  revolute  configuration,  caused  by 
interactive  torques  between  links.  In  order  that  this 
technique  to  be  effectively  used,  an  algorithm  must  be 
developed  which  will  take  into  account  the  initial 
orientation  of  the  arm  and  the  commanded  direction  of 
motion,  and  will  determine  the  magnitude  of  the  input  step 
command  and  the  optimum  delay  time. 

For  this  study  a  rigid  arm  was  assumed  for  both  robot 
configurations.  In  rigid  arms  the  speed  is  greatly  limited 
by  the  weight  of  the  arm.  This  weight  also  increases  the 
size  of  the  required  servo  motors  as  well  as  the  power 
consumption.  From  this  fact,  an  area  for  further  study  is  to 
model  an  arm  which  consists  of  either  only  flexible  links  or 
both  rigid  and  flexible  links. 
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DSL  PROGRAM  FOR  THE  SIMULATION  OF  THE  SECOND  ORDER  MODEL 


TITLE  THIS  PROGRAM  INVESTIGATES  THE  ABILITY  OF  THE  BASIC 
TITLE  MODEL  TO  FOLLOW  THE  CURVE  WHEN  VELOCITY  CURVE 
TITLE  FOLLOWING  TECHNIQUE  IS  USED. 

PARAM  K=1 . 0 , K1=0 . 6 , K2=10000 . 0 , KM=59 .29, VSAT=150 . 0 , R0=0 . 5 

PARAM  R=1 . 0 

INITIAL 

A=SQRT (2 . *KM*VSAT) 

CF=l./RO 
REF=R*CF 
XDOT=0 . 0 

DYNAMIC 

RTH=REF*STEP (0.0) 

E=RTH-C 

IF (E . LT . 0 . 0) XDOT=-A*Kl*SQRT ( ABS (E) ) 

IF (E.GE. 0 . 0) XD0T=A*K1*SQRT (E) 

DERIVATIVE 

KCDOT=K*  CDOT 

XDOTE=XDOT-KCDOT 

V=LIMIT ( -VSAT , VSAT , K2  *XDOTE ) 

CDDOT=V*KM 

CDOT=INTGRL (0.0, CDDOT) 

C«INTGRL( 0.0, CDOT) 

CX=C*1./CF 
XDOTX=XDOT*l . /CF 
CDOTX=CDOT* 1 . /CF 
TERMINAL 
METHOD  RKSFX 

CONTRL  FINTIM=0 . 1 , DELT=0 . 00005 
SAVE  (Sl)O.OOl, CX, XDOTX , CDOTX 
SAVE  (S2) 0.001, CX,R 

GRAPH (Gl/Sl , DE=TEK618 , PO=l , .5)  CX (LE=6 . 5 ,UN= ' INCHES ' )  ... 
XDOTX ( LE=8 , NI=8 , L0=0 . ,UN= ' INCHES/SEC ' , SC=$AR)  . . . 
CDOTX (LE=8 , NI=8 , L0=0 . , UN= ' INCHES/SEC ' , PO=6 . 5 , SC=$AR) 
GRAPH (G2/S2 , DE=TEK618 , PO=l , 1)  TIME ( LE=6 , UN= ' SEC ' ) , . . . 

CX ( LE=8 , NI=6 , LO=0 , UN= ' INCHES » ,SC=0.25) , . . . 

R(LE=8 , NI=6 , LO=0 , SC=0 .25, AX=OMIT) 

LABEL  (Gl)  PHASE  PLANE 

LABEL  (Gl)  XDOTX, CDOTX  VS  CX 

LABEL  (G2)  STEP  RESPONSE 

LABEL  (G2)  CX  VS  TIME 

END 

STOP 


DERIVATION  OF  MATHEMATICAL  MODEL  FOR  THE  THREE  LINK 

RECTANGULAR  ROBOT 

According  to  Newton's  equation  of  motion,  the  force 
applied  to  the  link  i,  by  the  joint  motor  i,  in  the 
direction  of  the  motion  of  the  link  i,  is 

Fi  -  Mi(vi-g)  (B.l) 

The  total  torque  about  the  axis  of  rotation  of  the  motor  i 
is  given  by  the  equation 

Ti  =  Fir+Jmi®i  (B-2) 

where  r  is  the  radius  of  the  pinion. 

Therefore  for  the  three  links  of  the  rectangular  robot, 


the  equations  of  motion  are 

F1  =  Ml*  (B. 3 ) 
f2  =  M2Y  (B. 4) 
F3  =  M3 (z-g)  (B. 5) 

and  the  corresponding  total  torques  are 

Tj.  -  Mixr+J^e!  (B.6) 
T2  =  M2yr+Jm2e2  (B. 7) 
T3  «  M3 (z-g) r+Jm303  (B. 8) 


But  the  linear  motion  of  the  link  i  is  related  to  the 


angular  motion  of  the  motor  i  with 


Si  =  Sir 


(B.9) 


Substituting  equation  B.9  into  equations  B.6,  B.7,  and  B.8 


TX  =  (MirZ+j^je-L 

t2  *  (M2r2+Jm2)®2 
T3  =  (M3r2+Jm3)e3-M3gr 


(B. 10) 
(B. 11) 
(B.12) 


DSL  PROGRAM  FOR  THE  THREE  LINK  RECTANGULAR  ROBOT 
UNDER  DIFFERENT  LOAD  CONDITIONS  (NO  GRAVITY) 
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TITLE  SIMULATION  PROGRAM  FOR  THE  RECTANGULAR  COORDINATE 
TITLE  CONFIGURATION  UNDER  DIFFERENT  LOAD  CONDITIONS 
PARAM  K»1 . 0 , K1*0 . 6 , K2=10000 . 0 , KM 1=5 9 . 29 , KM2=90. 2 5 , KM3=77 . 44 
PARAM  VSAT^ISO . 0,M1=0. 082  ,M2=0 . 041,M3=0 . 041  ,MM=s0 . 186 
PARAM  Jl-0. 033,J2=0.033,J3=0.033,R1=0.91,R2=0.91,R3=0.91 
PARAM  KT1=14.4,KT2=14.4,KT3=14.4,L1=.0001,L2=.0001,L3=.0001 
PARAM  BM1*0 . 04297 , BM2=0 . 04297 , BM3=0 . 04297 , R0=0 . 5 , LOAD=0 . 0 
PARAM  KV1=0. 1012, KV2=0. 1012 ,KV3=0. 1012, T=0. 00025 
PARAM  RX=1. 00  , RY=1. 00  ,RZ=1.00 
INTGER  SW1 , SW2 , SW3 , N1 , N2 , N3 

*  Kls  THE  CURVE  SCALING  CONSTANT 

*  K2:  THE  AMPLIFIER  GAIN 

*  KM:  THE  INITIAL  CONSTANT  OF  THE  IDEAL  (MODEL)  MOTOR 

*  VS AT:  THE  SATURATION  LIMITS  OF  THE  AMPLIFIER 

*  K:  THE  VELOCITY  LOOP  FEEDBACK  GAIN  (OF  THE  MODEL) 

*  RX , RY , RZ : THE  COMMANDED  TIP  POSITION  IN  INCHES 

*  RO:  THE  RADIUS  OF  THE  PINION  GEARING 

*  T:  THE  SAMPLING  INTERVAL 
INITIAL 

SW1=0 

SW2=0 

SW3«=0 

N1=*0 

N2=*0 

N3=0 

X1DOT=0 . 

X2DOT=0. 

X3DOT=0. 

TL1=0 . 

TL2=0 . 

TL3=0. 

CF«l./RO 

A1=SQRT(2. *KM1*VSAT) 

A2*SQRT ( 2 . *KM2 *VSAT) 

A3*SQRT ( 2 . *KM3*VSAT) 

REF1=RX*CF 

REF2=RY*CF 

REF3=RZ*CF 

DERIVATIVE 

TM1=M1+M2+M3+2*MM+L0AD 

TM2=M2+LOAD 

TM3=M2+M3+MM+LOAD 

JM1=TM1*R0 


210 


NOSORT 


SORT 


JM2=TM2*RO 

JM3=TM3*RO 

JT0T1=J1+JM1 

JTOT2  =  J  2  + JM2 

JTOT3«J3+JM3 

RTH1=REF1*STEP (0.0) 

RTH2=REF2 *STEP (0.0) 

RTH3=REF3 *STEP (0.0) 

E1=RTH1-C1 

E2-RTH2-C2 

E3=RTH3-C3 

ERX=RX-CRX 

ERY=RY-CRY 

ERZ=RZ-CRZ 

IF (El. LT. 0.0)  XI  DOT5 
IF (El. GE. 0.0)  XI DOT5 
IF (E2 . LT. 0.0)  X2DOT5 
IF (E2 . GE .0.0)  X2DOT5 
IF (E3 . LT .0.0)  X3DOT= 
IF (E3 . GE .0.0)  X3DOT5 


-A1*K1*SQRT ( ABS (El) ) 
A1*K1*SQRT (El) 
-A2*K1*SQRT (ABS (E2 )  ) 
‘A2*K1*SQRT(E2) 
=-A3*Kl*SQRT (ABS (E3) ) 
=A3*K1*SQRT(E3) 


************************************************************ 

KClDOT=K*ClDOT 

XlDOTE=XlDOT-KClDOT 

V1*LIMIT ( -VS AT , VS AT , K2  *X1D0TE ) 

C1DDT=V1*KM1 
C1DOT-INTGRL (0.0, C1DDT) 

Cl-INTGRL (0.0, C1DOT ) 

CX=C1*1./CF 
VM1=V1-KV1*CR1D0T 
MP1=REALPL (0.0, Ll/Rl , VM1/R1 ) 

MT1=KT1*MP1 

MT1E=MT1-BM1*CR1D0T-TL1 
CR1DDT= ( 1 . / JTOT1 ) *MT1E 
CR1D0T=INTGRL (0.0, CR1DDT) 

CR1*INTGRL (0.0, CR1DOT) 

CRX=CR1*1./CF 
XXDOT=X 1 DOT * 1 . / C F 
XXDOTE=X 1 DOTE * 1 . / C F 
CXDOT=C 1 DOT * 1 . / CF 
CRXDOT=CRlDOT*l./CF 

************************************************************ 

KC2DOT=K*C2DOT 

X  2 130TE= X  2  DOT  -  KC  2  DOT 

V2=LIMIT ( -VS AT , VS AT , K2  *  X  2  DOTE ) 

C2DDT=V2*KM2 


wwws 


C2 DOT-INTGRL ( 0 . 0 , C2  DDT ) 

C2«INTGRL(0.0,C2DOT) 

CY«C2*1./CF 

VM2-V2 -KV2 *CR2 DOT 

MP2-REALPL (0.0, L2/R2 , VM2/R2 ) 

MT2-KT2*MP2 

MT2E=MT2 -BM2 *CR2  DOT-TL2 
CR2DDT* ( 1 . / JTOT2 ) *MT2E 
CR2  DOT=INTGRL (0.0, CR2 DDT ) 
CR2=INTGRL (0.0, CR2DOT) 
CRY=CR2*1./CF 
XYDOT=X2 DOT* 1 . /CF 
XYDOTE=X2DOTE* 1 . /CF 
CYDOT=C2DOT*l./CF 
CRYDOT=CR2 DOT * 1 . / C F 


************************************************************ 


KC3  DOT*=K*C3  DOT 

X3 DOTE=X3 DOT-KC3 DOT 

V3=LIMIT ( -VSAT , VSAT , K2  *X3  DOTE ) 

C3DDT=V3*KM3 

C3 DOT=INTGRL (0.0, C3  DDT ) 

C3“INTGRL ( 0 . 0 , C3DOT) 

CZ«C3*1./CF 

VM3-V3 -KV3 - CR3 DOT 

MP3-REALPL (0.0, L3/R3 , VM3/R3 ) 

MT3-KT3*MP3 

MT3E*MT3 -BM3*CR3 DOT-TL3 
CR3 DDT=  ( 1 .  / JTOT3 )  *MT3 E 
CR3 DOT=INTGRL (0.0, CR3 DDT ) 
CR3-INTGRL (0.0, CR3 DOT) 
CRZ=CR3*1./CF 
XZDOT=X3DOT* 1 . /CF 
XZ  DOTE=X 3DOTE*l./CF 
CZDOT=C3DOT*l./CF 
CRZ DOT=CR3 DOT * 1 . / C F 


************************************************************ 


SAMPLE 

NOSORT 


20 


10 


IF  (N1.EQ.0.0)  GO  TO  10 

IF  (C1DOT.GT.X1DOT)  SW1=1 

IF  (SW1.EQ.1)  GO  TO  20 

KS1-ABS (2 . *CR1) / ( ( (N1*T) **2) *V1) 

KM1=KS 1 

CONTINUE 

C1=CR1 

C1D0T=CR1D0T 

N1=N1+1 
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CONTINUE 

IF  (N2.EQ.0.0)  GO  TO  30 

IF  (C2DOT.GT.X2DOT)  SW2=1 

IF  (SW2.EQ.1)  GO  TO  40 

KS2=ABS (2 . *CR2 ) / ( ( (N2*T) **2 ) *V2 ) 

KM2=KS2 

CONTINUE 

C2=CR2 

C2  DOT=CR2  DOT 

N2=N2+1 

CONTINUE 

IF  (N3.EQ.0.0)  GO  TO  50 

IF  (C3DOT.GT.X3DOT)  SW3=1 

IF  (SW3.EQ.1)  GO  TO  60 

KS3=ABS(2.*CR3)/( ( (N3*T) **2) *V3 ) 

KM3=KS3 

CONTINUE 

C3=CR3 

C3  DOT=CR3  DOT 

N3-N3+1 

CONTINUE 


SORT 


************************************************************ 

TERMINAL 
METHOD  RKSFX 

CONTRL  FINTIM=0 . 06 , DELT»0 . 00005 , DELS=0 .00025 

SAVE  (SI)  0 . 0004 , XXDOT, XYDOT, XZDOT , CXDOT, CYDOT , CZDOT, . . . 

CRXDOT , CRYDOT , CRZDOT , CRX , CRY , CRZ 
SAVE  (S2)  0.001, CX, CRX, RX, CY , CRY, RY, CZ , CRZ , RZ 
SAVE  (S3)  0 . 001 , ERX, ERY, ERZ 

GRAPH  ( Gl/S  1 ,  DE=TEK6 1 8 ,  PO= 1 )  CRX (LE=6 . 0 , UN= ' INCHES '),.. . 
CXDOT (NI=3 , L0=0 , SC=20,UN= ’ INCHES/SEC ' ) , . . . 

CRXDOT (NI=3 , L0=0 , PO=6 . 0 , SC=2 0 , UN= ' INCHES/SEC '),... 
XXDOT (NI=3 , LO=0 , SC=2  0 , AX=OMIT) 

GRAPH  (G2/S1, DE=TEK618 ,0V, PO=l, 3.25) . . . 

CRY (LE=6.0,UN=' INCHES') , . . . 

CYDOT (NI=3,LO=0,SC=2 5, UN= 1  INCHES/SEC ’ )  , . . . 

CRYDOT (NI=3 , L0=0 , PO=6 . 0 , SC=2 5 , UN= 1 INCHES/SEC ’),... 
XYDOT (NI=3 , L0=0 , SC=25 , AX=OMIT) 

GRAPH  (G3/S1 , DE=TEK618 , OV, P0=1 ,6.5) . . . 

CRZ ( LE=6 . 0 , UN= • INCHES  1 ) , . . . 

CZDOT (NI=3 , LO=0 , SC=2  0 , UN= • INCHES/SEC '),... 

CRZDOT (NI=3 , L0=0 , PO=6 . 0 , SC=2  0 , UN= • INCHES/SEC '),... 
XZDOT (NI=3 , L0=0 , SC=2  0 , AX=0MIT) 

GRAPH  (G4/S2 , DE=TEK618 , PO=l)  TIME (LE=6 . 0 , NI=6 , UN= 1  SEC '),.. . 
CX(NI=3,LO=0.0,UN=' INCHES' , SC=0 . 5 , PO=6 . 0) , . . . 

CRX (NI=3,LO=0.0,UN=' INCHES' ,SC=0.5) , . . . 

RX (NI=3 , LO=0 . 0 , SC=0 . 5 , AX=OMIT) 

GRAPH  (G5/S2 , DE=TEK618 , OV, PO=l , 3.25) . . . 


V’J^V 


_ _ ' 


-  /TT’CT.V.V.V^CT.V  V  V  V 


GRAPH 


GRAPH 


LABEL 

LABEL 

LABEL 

END 

STOP 


TIME (LE=6. 0^1*6^=’ SEC')  ,  .  .  . 

CY (NI=3 , LO=0 . 0 , UN= ' INCHES ' , SC=0 . 5 , PO=6 . 0 ) , . . . 

CRY (NI=3,LO=0.0,UN=' INCHES' ,SC=0.5) ,  .  .  . 

RY (NI=3 , L0=0 . 0 , SC=0 . 5 , AX=OMIT) 

(G6/S2 , DE=TEK618 ,0V, P0=1, 6.5) . . . 

TIME (LE=6. 0,NI=6,UN=' SEC ' ) , . . . 

CZ  (NI-3  ,  L0=0 . 0 ,  UN=  1  INCHES  '  ,  SC=0 . 5 ,  P0=6 . 0)  ,  .  .  . 

CRZ (NI=3 , LO=0 . 0 , UN= ' INCHES ' ,SC=0.5) ,  .  .  . 

RZ (VI— 3 , LO—O . 0 , SC=0 . 5 , AX=0MIT) 

(G7/S3 , DE=TEK618 , PO=l , .5) . . . 

TIME (LE=5.0,NI=6,UN=' SEC ' ) , . . . 

ERX ( LE=8 , NI=10 , L0=0 . 0 , UN= ' INCHES ' , SC= . 1 ) , . . . 

ERY ( LE=8 , NI=10 , L0=0 . 0 , UN= 1 INCHES ' , SC= . 1 , PO=5 . 0 ) , 
ERZ (LE=8 , NI=10 , LO=0 . 0 , UN- ' INCHES ' , SC= . 1 , PO=6 . 2 ) 
(Gl)  PHASE  PLANE  ( CDOT , CRDOT , XDOT . VS . CR) 

(G4 )  STEP  RESPONSE 
(G7)  ERROR  .VS.  TIME 


DSL  PROGRAM  FOR  THE  THREE  LINK  RECTANGULAR  ROBOT 
WITH  DISTURBANCE  (NO  GRAVITY) 


TITLE  SIMULATION  PROGRAM  FOR  THE  RECTANGULAR  COORDINATE 
TITLE  CONFIGURATION  WITH  DISTURBANCE  (NO  GRAVITY) 

PARAM  K*l. 0, K1=0. 6,K2=10000 . 0 , KM1=59 . 29 ,KM2=90 . 25 , KM3=77 . 44 
PARAM  VSAT=150.0,M1=0.082,M2=0.041,M3=0.041,MM=0. 186 
PARAM  Jl=0.033, J2=0.033, J3=0 . 033 , R1=0 . 91 , R2=0 . 91 , R3=0 . 91 
PARAM  KT1=14 . 4 , KT2=14 . 4 , KT3=14 . 4 , Ll= . 0001 , L2= . 0001 , L3= . 0001 
PARAM  BM1=0 . 04297 , BM2=0 . 04297 , BM3=0 . 04297 , R0=0 . 5 , LOAD=0 . 0 
PARAM  KV1=0 . 1012 , KV2=0 . 1012 , KV3=0 . 1012 , T=0 .00025 
PARAM  RX=1.00  , RY=1 . 00  ,RZ=1.00 
INTGER  SW1.-SW2,SW3^N1,N2,,N3_ 

*  Kl:  THE  CURVE  SCALING  CONSTANT 

*  K2 :  THE  AMPLIFIER  GAIN 

*  KM:  THE  INITIAL  CONSTANT  OF  THE  IDEAL  (MODEL)  MOTOR 

*  VSAT:  THE  SATURATION  LIMITS  OF  THE  AMPLIFIER 

*  K:  THE  VELOCITY  LOOP  FEEDBACK  GAIN  (OF  THE  MODEL) 

*  RX , RY , RZ : THE  COMMANDED  TIP  POSITION  IN  INCHES 

*  RO:  THE  RADIUS  OF  THE  PINION  GEARING 

*  T:  THE  SAMPLING  INTERVAL 
INITIAL 

SW1=0 

SW2— 0 

SW3=0 

N1=0 

N2=0 

N3=0 

X1DOT=0 . 

X2DOT=0. 

X3DOT— 0 . 

TL1=0 . 

TL2=0 . 

TL3=0 . 

CF«=1  ./RO 

A1=SQRT ( 2 . *KM1*VSAT) 

A2*SQRT ( 2 . *KM2 *VSAT) 

A3=SQRT ( 2 . *KM3  *VSAT) 

REF1=RX*CF 

REF2=RY*CF 

REF3=RZ*CF 

DERIVATIVE 

TMl=Ml+M2+M3  +  2  *MM+LOAD 

TM2=M2+LOAD 

TM3 =M2 +M3 +MM+ LOAD 

JMl=TMl*RO 


NOSORT 


JM2-TM2*RO 
JM3=TM3*RO 
JTOT 1=J 1+ JM1 
JTOT2*J2+JM2 
JTOT3=J3+JM3 

RTH1=REF1*STEP(0 . 0) +10* (STEP(0 . 010) -STEP(0 . 012) ) 

RTH2=REF2*STEP( 0.0) +10* (STEP (0.010) -STEP (0.012) ) 

RTH3=REF3*STEP (0.0) +10* (STEP (0.010) -STEP (0.012) ) 

E1=RTH1-C1 

E2=RTH2-C2 

E3*RTH3-C3 

IF (El . LT. 0.0)  X1D0T=-A1*K1*SQRT(ABS(E1) ) 

IF (El . GE . 0 . 0 )  X1D0T=A1*K1*SQRT (El) 

IF (E2 . LT. 0.0)  X2D0T=-A2*K1*SQRT (ABS (E2 ) ) 

IF (E2 . GE . 0 . 0)  X2DOT=A2 *K1*SQRT (E2 ) 

IF (E3 . LT. 0.0)  X3D0T=-A3*K1*SQRT (ABS (E3 ) ) 

IF (E3 . GE .0.0)  X3D0T=A3*K1*SQRT(E3) 


SORT 


************************************************************ 

KClDOT=K*ClDOT 

XlDOTE=XlDOT-KClDOT 

V1=LIMIT ( -VSAT , VSAT , K2+X1DOTE ) 

C1DDT=V1*KM1 
C1D0T=INTGRL (0.0, C1DDT) 

C1*=INTGRL  (0.0,  C1DOT) 

CX=C1*1./CF 
VM1=*V1-KV1*CR1D0T 
MP1=REALPL (0.0, Ll/Rl , VM1/R1 ) 

MT1=KT1*MP1 

MT 1E=MT 1- BM1 * CR1 DOT-TL1 
CR1DDT= ( 1 . / JTOT1 ) *MT1E 
CR1D0T=INTGRL (0.0, CR1DDT) 

CR1=INTGRL (0.0, CR1DOT ) 

CRX=CR1*1./CF 
XXD0T=X1D0T*1 . /CF 
CXD0T=C1D0T* 1 . / CF 
CRXDOT=CR 1 DOT * 1 . /CF 

************************************************************ 

KC2DOT=K*C2DOT 

X2 DOTE=X2 DOT -KC2  DOT 

V2=LIMIT ( -VSAT , VSAT , K2  *X2  DOTE ) 

C2DDT=V2*KM2 
C2DOT=INTGRL (0.0, C2DDT) 

C2=INTGRL( 0 . 0 , C2DOT) 

CY=C2*1./CF 
VM2=V2 -KV2 *CR2 DOT 


B 


MP2=eREALPL (0.0, L2/R2 , VM2/R2 ) 

MT2=KT2  *MP2 

MT2E=xMT2-BM2*CR2DOT-TL2 
CR2DDT* ( 1 ./JTOT2 ) *MT2E 
CR2 DOT=INTGRL (0.0, CR2 DDT) 

CR2=INTGRL (0.0, CR2 DOT) 

CRY=CR2  * 1 . /CF 
XYDOT=X2DOT*l . /CF 
CYDOT=C2DOT*l./CF 
CRYDOT=CR2 DOT* 1 . /CF 

************************************************************ 

KC3DOT=K*C3DOT 

X3 DOTE=X3 DOT-KC3 DOT 

V3=LIMIT ( -VS AT , VSAT , K2  *X3  DOTE ) 

C3DDT=V3*KM3 
C3 DOT=INTGRL (0.0, C3  DDT ) 

C3=INTGRL (0.0, C3  DOT) 

CZ=C3*1./CF 
VM3=V3 -KV3 *CR3 DOT 
MP3=REALPL (0.0, L3/R3 , VM3/R3 ) 

MT3=KT3  *MP3 

MT3E=MT3-BM3*CR3DOT-TL3 
CR3DDT= ( 1 . /JTOT3 ) *MT3E 
CR3 DOT=INTGRL (0.0, CR3 DDT ) 

CR3 =INTGRL (0.0, CR3 DOT ) 

CRZ=CR3*1./CF 
XZ DOT=X3 DOT * 1 . / CF 
CZDOT=C3DOT*l./CF 
CRZ DOT=CR3  DOT * 1 . / CF 

************************************************************ 

SAMFLE 

NOSORT 

IF  (Nl.EQ.0.0)  GO  TO  10 
IF  ( C1DOT . GT . X1DOT )  SW1»1 
IF  (SW1.EQ.1)  GO  TO  20 
KS1=ABS ( 2 . *CR1) / ( ( (N1*T) **2) *V1) 

KM1=KS1 

20  CONTINUE 

C1=CR1 

C 1 DOT=CR 1 DOT 
10  N1=N1+1 

CONTINUE 

IF  (N2.EQ.0.0)  GO  TO  30 
IF  (C2DOT.GT. X2DOT)  SW2=1 
IF  (SW2.EQ.1)  GO  TO  40 
KS2=ABS ( 2 . *CR2 ) / ( ( (N2*T) **2) *V2) 

KM2=KS2 


'  ' i'j' l'«l«  >'  •'  ‘  .  • 


40  CONTINUE 

C2=CR2 

C2  DOT=CR2  DOT 
30  N2=N2+1 

CONTINUE 

IF  (N3.EQ.0.0)  GO  TO  50 
IF  (C3DOT.GT.X3DOT)  SW3=1 
IF  (SW3.EQ.1)  GO  TO  60 
KS3=ABS (2 . *CR3 ) / ( ( (N3*T) **2) *V3) 

KM3=KS3 

60  CONTINUE 

C3=CR3 

C3  DOT=CR3  DOT 
50  N3=N3+1 

CONTINUE 

SORT 

a*********************************************************** 

TERMINAL 
METHOD  RKSFX 

CONTRL  FINTIM=0 .  08 , DELT=0 . 00005 , DELS=0 .00025 

SAVE  (SI)  0 . 0001 , XXDOT, XYDOT, XZDOT, CXDOT, CYDOT, CZDOT, . . . 

CRXDOT , CRYDOT , CRZDOT , CRX , CRY , CRZ 
SAVE  (S2)  0.001/ CX , CRX , RX , CY , CRY , RY , CZ , CRZ , RZ 
GRAPH  (Gl/Sl , DE=TEK618 , PO=l) . . . 

CRX (LE=6 . 0 , NI— 10 , UN= ' INCHES '),... 

CXDOT (NI=3 , L0=0 , SC=3  0 , UN= ' INCHES/SEC '),... 

CRXDOT  (NI5®!  ,  L0=0 ,  PO*=6 . 0 , 80=3 0, UN—  *  INCHES/SEC  1  )  ,  .  .  . 
XXDOT (NI=3 , LO=0 , SC=30 , AX=OMIT) 

GRAPH  (G2/S1 , DE=TEK618 , OV, PO=l ,3.25) . . . 

CRY (LE=6.0,NI=10,UN=' INCHES') , . . . 

CYDOT (NI=3 , LO=0 , SC=3  0 , UN= ' INCHES/SEC '),... 

CRYDOT (NI=3 , L0=0 , PO=6 . 0 , SC=3 0 , UN= ' INCHES/SEC '),... 
XYDOT (NI=3 , L0=0 , SC=30 , AX=OMIT) 

GRAPH  (G3/S1 , DE=TEK618 , OV, PO=l ,6.5) . . . 

CRZ (LE=6.0,NI=10,UN=' INCHES') ,... 

CZDOT (NI=3 , L0=0 , SC=3  0 , UN= » INCHES/SEC ’),... 

CRZDOT (NI=3 , LO=0 , P0=6 . 0 , SC=3  0 , UN= ' INCHES/SEC '),... 
XZDOT (NI=3 , L0=0 , SC=30 , AX=0MIT) 

GRAPH  (G4/S2 , DE=TEK618 , P0=1)  TIME (LE=6 . 0 , NI=8 , UN= ' SEC '),.. . 
CX(NI=3,LO=0.0,UN=' INCHES' , SC=0 . 5 , PO=6 . 0) , . . . 

CRX (NI=3,LO=0.0,UN=' INCHES' ,SC=0.5) , . . . 

RX (NI=3 , L0=0 . 0 , SC=0 . 5 , AX=0MIT) 

GRAPH  (G5/S2  ,  DE=TEK618 ,  OV,.  P0=1, 3.25)  .  .  . 

TIME (LE=6.0,NI=8,UN=' SEC ' ) , . . . 

CY (NI=3 , LO=0 . 0 , UN= ' INCHES ' , SC=0 . 5 , PO=6 . 0 ) , . . . 

CRY (NI=3,LO=0.0,UN=' INCHES' ,SC=0.5) , . . . 

RY (NI=3 , LO=0 . 0 , SC=0 . 5 , AX=OMIT) 

GRAPH  (G6/S2 , DE=TEK618 ,0V, PO=l , 6.5) . . . 

TIME (LE=6.0,NI=8,UN=' SEC ' ) , . . . 


CZ (NI=3,LO»0.0,UN»' INCHES ' ,SC=0. 5, PO=6.0) , . . . 
CRZ (NI=3 , LO=0 . 0 , UN= ' INCHES ' ,SC=0.5) , . . . 

RZ (NI=3 , L0=0 . 0 , SC=0 . 5 , AX=OMIT) 

LABEL  (Gl)  PHASE  PLANE  (COOT, CRDOT, XDOT . VS . CR) 

LABEL  (G4 )  STEP  RESPONSE 

END 

STOP 


************************************************************ 


DSL  PROGRAM  FOR  THE  THRE  LINK  RECTANGULAR  ROBOT 
UNDER  DIFFERENT  LOAD  CONDITIONS 
(GRAVITATIONAL  TORQUES  INCLUDED) 


TITLE  SIMULATION  PROGRAM  FOR  THE  RECTANGULAR  COORDINATE 
TITLE  CONFIGURATION  UNDER  DIFFERENT  LOAD  CONDITIONS 
TITLE  (WITH  GRAVITY) 

PARAM  K=1 . 0 , K1=0 . 6 , K2=10000 . 0 , KM1=59 . 29 , KM2=90. 25 , KM3=77 . 44 
PARAM  VSAT=150 . 0 , M1=0 . 082 , M2=0 .041, M3=0 .041, MM=0 . 186 
PARAM  J1=0 .033 , J2=0 .033 , J3=0 . 033 , R1=0 . 91 , R2=0 . 91 , R3=0 . 91 
PARAM  KT1=14 . 4 , KT2=14 . 4 , KT3=14 . 4 , Ll= . 0001 , L2= . 0001, L3=. 0001 
PARAM  BM1=0 . 04297 , BM2=0 . 04297 , BM3=0 . 04297 ,RO=0 . 5 , LOAD=0 .86 
PARAM  KV1=0. 1012, KV2=0. 1012, KV3=0. 1012, T=0. 00025, G=386. 4 
PARAM  RX=1 . 00  , RY=1 . 00  ,RZ=1.00 
INTGER  SW1 , SW2 , SW3 , N1 , N2 , N3 

*  Kl:  THE  CURVE  SCALLING  CONSTANT 

*  K2:  THE  AMPLIFIER  GAIN 

*  KM:  THE  INITIAL  CONSTANT  OF  THE  IDEAL  (MODEL)  MOTOR 

*  VSAT:  THE  SATURATION  LIMITS  OF  THE  AMPLIFIER 

*  K:  THE  VELOCITY  LOOP  FEEDBACK  GAIN  (OF  THE  MODEL) 

*  RX , RY , RZ :  THE  COMMANDED  TIP  POSITION  IN  INCHES 

*  RO:  THE  RADIUS  OF  THE  PINION  GEARING 

*  T:  THE  SAMPLING  INTERVAL 
INITIAL 

SW1=0 

SW2=0 

SW3=0 

N1=0 

N2=0 

N3=0 

X1DOT=0 . 

X2DOT=0 . 

X3DOT=0. 

TL1=0. 

TL2=0 . 

TL3= (M2+M3+MM) *G*RO 
CF=l./RO 

A1=SQRT ( 2 . *KM1 *VSAT) 

A2=SQRT ( 2 . *KM2 *VSAT) 

A3=SQRT ( 2 . *KM3 *VSAT) 

REF1=RX*CF 

REF2=RY*CF 

REF3=RZ*CF 

DERIVATIVE 

TM1=M1+M2+M3+2*MM+L0AD 

TM2=M2+LOAD 


TM3 “M2 +M3  +MM+ LOAD 
JM1=TM1*R0 
JM2=TM2*RO 
JM3=TM3*RO 
JT0T1=J1+JM1 
JTOT2  =J  2  + JM2 
JTOT3=J3+JM3 
RTH1=REF1 *STEP (0.0) 

RTH2=REF2 *STEP (0.0) 

RTH3=REF3 *STEP (0.0) 

E1=RTH1-C1 
E2*RTH2-C2 
E3=RTH3-C3 
ERX— RX - CRX 
ERY=RY-CRY 
ERZ=RZ-CRZ 

NOSORT 

IF (El . LT .0.0)  X1D0T=-A1*K1*SQRT (ABS (El) ) 

IF (El . GE . 0 . 0)  X1D0T=A1*K1*SQRT (El) 

IF (E2 . LT .0.0)  X2D0T=-A2*K1*SQRT(ABS(E2) ) 

IF ( E2 . GE . 0 . 0 )  X2 DOT=A2  *K1*SQRT ( E2 ) 

IF (E3 . LT. 0.0)  X3D0T=-A3*K1*SQRT(ABS(E3) ) 

IF  (E3  .  GE .  0 . 0 )  X3IX5T— A3  *K1*SQRT  ( E3 ) 

SORT 

i ********************************************************** ** 

KClDOT=K*ClDOT 

XlDOTE=XlDOT-KClDOT 

V1=LIMIT ( -VSAT , VSAT , K2  *X1D0TE ) 

C1DDT=V1*KM1 
C1D0T=INTGRL (0.0, C1DDT) 

C1=INTGRL (0.0, C1DOT) 

CX=C1*1 ./CF 
VM1=V1-KV1*CR1D0T 
MP1=REALPL (0.0, Ll/Rl , VH1/R1 ) 

MT1=KT1*MP1 

MT1E=MT1-BM1*CR1D0T-TL1 
CR1DDT= ( 1 . /JTOT1) *MT1E 
CR1D0T=INTGRL (0.0, CR1DDT) 

CR1=INTGRL (0.0, CR1DOT) 

CRX=CR1*1./CF 
XXDOT=X 1 DOT * 1 . /CF 
XXD0TE=X1D0TE* 1 . /CF 
CXDOT=ClDOT* 1 . /CF 
CRXDOT=CRlDOT*l./CF 

************************************************************ 

KC2DOT=K*C2DOT 
X2 DOTE=X2  DOT-KC 2  DOT 


V2=LIMIT ( -VSAT , VS AT , K2  *X2 DOTE ) 

C2DDT=V2*KM2 
C2 DOT=INTGRL ( 0 . 0 , C2  DDT ) 

C2 =INTGRL ( 0 . 0 , C2  DOT ) 

CY=C2*1 o/CF 
VM2=V2 -KV2 *CR2 DOT 
MP2=REALPL ( 0 . 0 , L2 /R2 , VM2/R2 ) 

MT2=KT2*MP2 

MT2E=MT2 -BM2 *CR2  DOT-TL2 
CR2 DDT= ( 1 . / JT0T2 ) *MT2  E 
CR2  DOT=INTGRL (0.0, CR2  DDT ) 

CR2=INTGRL (0.0, CR2  DOT ) 

CRY=CR2*1./CF 
XYDOT=X2DOT*l./CF 
XYDOTE=X2 DOTE  * 1 . / CF 
CYDOT=C2DOT*l./CF 
CRYDOT=CR2DOT*l ./CF 

************************************************************ 

KC3DOT=K*C3DOT 

X3 DOTE=X3  DOT-KC3 DOT 

V3=LIMIT ( -VSAT , VSAT , K2  *X3  DOTE ) 

C3DDT=V3*KM3 
C3 DOT=INTGRL ( 0 . 0 , C3 DDT ) 

C3=INTGRL ( 0 . 0 , C3  DOT ) 

CZ=C3*1./CF 

VM3 =V3 -KV3 *CR3 DOT 

MP3«REALPL (0.0, L3/R3 , VM3/R3 ) 

MT3=KT3*MP3 

MT3E=MT3 -BM3 *CR3 DOT-TL3 
CR3DDT= ( 1 . /JTOT3 ) *MT3E 
CR3 DOT=INTGRL (0.0, CR3 DDT) 

CR3=INTGRL (0.0, CR3 DOT ) 

CRZ=CR3  * 1 . /CF 
XZDOT=X3DOT*l./CF 
XZDOTE=X3 DOTE  * 1 . / CF 
CZDOT=C3 DOT* 1 . /CF 
CRZ DOT=CR3 DOT* 1 . / CF 

************************************************************ 

SAMPLE 

NOSORT 

IF  (N1.EQ.0.0)  GO  TO  10 
IF  (C1DOT . GT . X1DOT)  SW1=1 
IF  (SW1.EQ.1)  GO  TO  20 
KS1=ABS ( 2 . *CR1) / ( ( (N1*T) **2) *V1) 

KM1=KS1 

20  CONTINUE 

C1=CR1 


222 


W.V 


A 


C1D0T=CR1D0T 

N1=N1+1 

CONTINUE 

IF  (N2.EQ.0.0)  GO  TO  30 

IF  (C2DOT.GT.X2DOT)  SW2=1 

IF  (SW2.EQ.1)  GO  TO  40 

KS2=ABS (2 . *CR2 ) / ( ( (N2*T) **2) *V2) 

KM2=KS2 

CONTINUE 

C2=CR2 

C2  DOT=CR2  DOT 

N2=N2+1 

CONTINUE 

IF  (N3.EQ.0.0)  GO  TO  50 

IF  (C3DOT.GT. X3DOT)  SW3=1 

IF  (SW3.EQ.1)  GO  TO  60 

KS3=ABS(2.*CR3)/( ( (N3*T) **2) *V3) 

KM3=KS3 

CONTINUE 

C3=CR3 

C3  DOT=CR3  DOT 

N3=N3+1 

CONTINUE 


SORT 


************************************************************ 

TERMINAL 
METHOD  RKSFX 

CONTRL  FINTIM=0 . 1 , DELT=0 . 00005 , DELS=0 . 00025 

SAVE  (SI)  0.0004, XXDOT , XYDOT , XZDOT , CXDOT , CYDOT , CZDOT , . . . 

CRXDOT , CRYDOT , CRZDOT , CRX , CRY , CRZ 
SAVE  (S2)  0 . 001, CX, CRX, RX, CY , CRY, RY , CZ , CRZ , RZ 
SAVE  (S3)  0. 001,ERX,ERY,ERZ 
GRAPH  (Gl/Sl , DE=TEK618 , PO=l ) . . . 

CRX ( LE=6 . 0 , NI=6 , UN= ' INCHES ' ) , . . . 

CXDOT (NI=3,LO=-30,SC=30,UN=' INCHES/SEC ' ) , . . . 

CRXDOT (NI=3 , LO=-3  0 , PO=6 . 0 , SC=3  0 , UN= ' INCHES/SEC '),... 
XXDOT (NI=3 , LO=-3  0 , SC=30 , AX=OMIT) 

GRAPH  (G2/S1 , DE=TEK618 , OV, PO=l ,3.25) . . . 

CRY (LE=6.0,NI=6,UN=' INCHES ' ) , . . . 

CYDOT (NI=3 , LO=-3 5 , SC=3  5 , UN= ' INCHES/SEC '),... 

CRYDOT (NI=3,LO=-3 5, PO=6.0,SC=3 5, UN= ' INCHES/SEC ' ) , . . . 
XYDOT (NI=3 , LO=-3  5 , SC=35 , AX=OMIT) 

GRAPH  (G3/S1 , DE=TEK618 , OV, PO=l , 6.5) . . . 

CRZ (LE=6.0,NI=6,UN=' INCHES' ) , . . . 

CZDOT (NI=3,LO=-30,SC=30,UN=' INCHES/SEC ' ) , . . . 

CRZDOT (NI=3 , L0=-30 , PO=6 . 0 , SC=30 ,UN= ' INCHES/ SEC ' ) , . . . 
XZDOT (NI=3 , LO=-3  0 , SC=3  0 , AX=OMIT) 

GRAPH  (G4/S2 , DE=TEK618 , PO=l)  TIME (LE=6 . 0 , NI=5 , UN= ' SEC '),.. . 
CX (NI=3 , LO=0 . 0 , UN= ' INCHES ' , SC=0 . 5 , PO=6 . 0 ) , . . . 


/.A*  .V  - 


O  O  a*  \.V  \ 


CRX(NI«3,LO=0.0,UN“' INCHES' ,SC=0.5) ,  .  .  . 

RX {NI=3 , L0=0 . 0 , SC=0 . 5 , AX=OMIT) 

GRAPH  (G5/S2 , DE*TEK618 , OV, PO=l ,3.25) . . . 

TIME (LE=6.0,NI=*5,UN=' SEC '  )  ,  .  .  . 

CY  (NI=3  ,  L0=0 . 0  ,UN=*  1  INCHES  1  ,  SC=0 . 5 ,  PO=6 . 0)  ,  .  .  . 

CRY (NI«3/LO*0.0,UN=' INCHES  * ,SC=0.5)  , . . . 

RY (NI=3 , L0=0 . 0 , SC=0 . 5 , AX=OMIT) 

GRAPH  (G6/S2 , DE=TEK618 , OV, PO=l ,6.5) . . . 

TIME (LE=6 . 0 , NI=5 ,UN= ' SEC ' ) , . . . 

CZ (NI=3 , L0=0 . 0 , UN= ' INCHES  * , SC=0 . 5 , PO=6 . 0) , . . . 

CRZ  (NI=3  ,  LO»=0 . 0 ,  UN=  1  INCHES  1  ,SC=0.5)  ,  .  .  . 

RZ (NI=3 , L0=0 . 0 , SC=0 . 5 , AX=OMIT) 

GRAPH  (G7/S3 , DE=TEK618 , PO=l , .5) . . . 

TIME (LE=5 . 0 , NI=5 , UN= ' SEC ' ) , . . . 

ERX ( LE=8 , NI=6 , LO=- . 2 , UN= ' INCHES  1 , SC=. 2)  , . . . 

ERY ( LE=8 , NI=6 , LO=- . 2 , UN= ' INCHES ' , SC= . 2 , PO=5 . 0 ) , . . . 

ERZ ( LE— 8 , NI=6 , LO=- . 2 , UN= ' INCHES ' , SC= . 2 , PO=6 . 2 ) 
LABEL  (Gl)  PHASE  PLANE  ( CDOT , CRDOT , XDOT . VS . CR) 

LABEL  (G4 )  STEP  RESPONSE 
LABEL  (G7 )  ERROR  .VS.  TIME 
END 
STOP 
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DSL  PROGRAM  FOR  THE  THREE  LINK  RECTANGULAR  ROBOT  WITH 
DISTURBANCE  (GRAVITATIONAL  TORQUES  INCLUDED) 


TITLE  SIMULATION  PROGRAM  FOR  THE  RECTANGULAR  COORDINATE 
TITLE  CONFIGURATION  WITH  DISTURBANCE  (WITH  GRAVITY) 

PARAM  K=1 . 0 , K1=0 . 6 , K2=10000 . 0 , KM1=59 . 29 , KM2=9 0.25, KM3=77 . 44 
PARAM  VSAT=150 . 0 ,M1=0 .082 ,M2=0 . 041 ,M3=0 . 041 , MM=0 .186 
PARAM  Jl=0.033, J2=0 . 033 , J3=0 . 033 , R1=0 . 91 , R2=0 . 91 , R3=0 . 91 
PARAM  KT1=14 . 4 , KT2=14 . 4 , KT3=14 . 4 , Ll= . 0001, L2=. 0001, L3= .0001 
PARAM  BM1»0. 04297, BM2=0. 04297, BM3=0. 04297, RO=0. 5, LOAD=0.0 
PARAM  KV1=0. 1012, KV2=0. 1012, KV3=0. 1012, T=0. 00025, G=386. 4 
PARAM  RX=1. 00  ,  RY=*1 . 00  ,RZ=1.00 
INTGER  SW1 , SW2 , SW3 , N1 , N2 , N3 

*  Kl:  THE  CURVE  SCALLING  CONSTANT 

*  K2:  THE  AMPLIFIER  GAIN 

*  KM:  THE  INITIAL  CONSTANT  OF  THE  IDEAL  (MODEL)  MOTOR 

*  VSAT:  THE  SATURATION  LIMITS  OF  THE  AMPLIFIER 

*  K:  THE  VELOCITY  LOOP  FEEDBACK  GAIN  (OF  THE  MODEL) 

*  RX , RY , RZ : THE  COMMANDED  TIP  POSITION  IN  INCHES 

*  RO:  THE  RADIUS  OF  THE  PINION  GEARING 

*  T:  THE  SAMPLING  INTERVAL 
INITIAL 

SW1=0 

SW2=0 

SW3=0 

N1=0 

N2=0 

N3=0 

X1DOT=0. 

X2DOT=0 . 

X3DOT=0 . 

TL1=0. 

TL2=0. 

TL3= (M2+M3+MM) *G*RO 
CF=l./RO 

AI*SQRT ( 2 . *KM1*VSAT) 

A2-SQRT ( 2 . *KM2  *VSAT) 

A3*SQRT ( 2 . *KM3 *VSAT) 

REF1=RX*CF 

REF2=RY*CF 

REF3=RZ*CF 

DERIVATIVE 

TMl=Ml+M2+M3+2  *MM+LOAD 

TM2*M2+LOAD 

TM3 =M2 +M3  +MM+LOAD 

JMl=TMl*RO 


JM2=TM2*RO 
JM3=TM3*RO 
JT0T1=J1+JM1 
JT0T2  =  J  2  + JM2 
JTOT3=J3+JM3 

RTH1=REF1*STEP ( 0 . 0) +10* ( STEP (0.050) -STEP (0.052)) 

RTH2=REF2*STEP (0 . 0) +10* (STEP ( 0 . 050) -STEP ( 0 . 052 ) ) 

RTH3=REF3*STEP (0.0) +10* (STEP (0.050) -STEP (0.052) ) 

E1=RTH1-C1 

E2=RTH2-C2 

E3*=RTH3-C3 

NOSORT 

IF (El. LT. 0.0)  X1D0T=-A1*K1*SQRT (ABS (El) ) 

IF (El .GE . 0 . 0)  X1D0T=A1*K1*SQRT (El ) 

IF (E2 . LT. 0.0)  X2D0T=-A2*K1*SQRT (ABS (E2 ) ) 

IF (E2 .GE. 0.0)  X2D0T=A2*K1*SQRT(E2) 

IF (E3 . LT. 0.0)  X3DOT=-A3  *K1*SQRT (ABS (E3 ) ) 

IF (E3 .GE. 0. 0)  X3D0T=A3*K1*SQRT(E3) 

SORT 

it*********************************************************** 

KC1D0T=K*C1D0T 

X1D0TE=X1D0T-KC1D0T 

V1=LIMIT ( -VS AT , VS AT , K2  *X1D0TE ) 

C1DDT=V1*KM1 
C 1  DOT=*  INTGRL (0.0,  C1DDT) 

C1=INTGRL (0.0, C1DOT) 

CX*C1*1./CF 
VMl=Vl-KVl*CRlDOT 
MP1*=REALPL  (0.0,  Ll/Rl ,  VM1/R1 ) 

MT1=KT1*MP1 

MT1E=MT1-BM1*CR1D0T-TL1 
CR1DDT= ( 1 . / JTOT1 ) *MT1E 
CR1D0T=INTGRL (0.0, CR1DDT) 

CR1=INTGRL (0.0, CR1DOT) 

CRX=CR1*1./CF 
XXDOT=X 1 DOT * 1 . /CF 
CXDOT=ClDOT*l . /CF 
CRXOOT=CR 1 DOT  * 1 . /CF 

*********************************  ******************* ******** 

KC2DOT=K*C2DOT 

X2 1X)TE=X2  DOT-KC2  DOT 

V2=LIMIT ( - VSAT , VS AT , K2  *X2  DOTE ) 

C2DDT=V2*KM2 
C2 DOT=INTGRL ( 0 . 0 , C2  DDT ) 

C2=INTGRL(0.0,C2DOT) 

CY=C2*1./CF 
VM2*V2 -KV2 *CR2 DOT 


MF2-REALPL  (0.0,L2/R2,  VM2/R2 ) 

MT2-KT2*MP2 

MT2E«MT2-BM2*CR2DOT-TL2 
CR2DDT— ( 1 . /JT0T2 ) *MT2E 
CR2D0T-INTGRL (0.0, CR2DDT) 

CR2-INT6RL ( 0 . 0 , CR2DOT) 

CRY-CR2*1./CF 
XYDOT-X2DOT*l ./CF 
CYDOT-C2 DOT* 1 . /CF 
CRYDOT-CR2DOT*l./CF 

************************************************************ 


RC3  DOT— K*C3  DOT 

X3DOTE-X3 DOT-KC3 DOT 

V3 -LIMIT ( -VSAT , VSAT , K2  *X3D0TE) 

C3DDT— V3  *KM3 

C3DOT— INTGRL ( 0 . 0 , C3DDT) 

C3-INTGRL ( 0 . 0 , C3 DOT ) 

CZ— C3*1./CF 

VM3-V3-KV3 *CR3DOT 

MP3 -REALP L (0.0, L3/R3 , VM3/R3 ) 

MT3— KT3  *MP3 

MT3 E-MT3 -BM3 *CR3 DOT-TL3 
CR3DDT- ( 1 . / JTOT3 ) *MP3E 
CR3 DOT— INTGRL (0. 0,CR3 DDT) 

CR3— INTGRL (0.0, CR3 DOT) 

CRZ— CR3*1./CF 
XZDOT-X3 DOT* 1 . /CF 
CZDOT— C3DOT*l . /CF 
CRZDOT— CR3 DOT* I . /CF 

************************************************************ 


SAMPLE 

NOSORT 

IF  (N1.SQ.0.0)  GO  TO  10 
IF  (C1DOT.GT.X1DOT)  SW1-1 
IF  (SN1.EQ.1)  GO  TO  20 
KS1-ABS (2 . *CR1)/ ( ( (N1*T) **2) *V1) 
KM1-KS1 

20  CONTINUE 

C1-CR1 

C1DOT— CR1DOT 
10  Nl-Nl+1 

CONTINUE 

IF  (N2.EQ.0.0)  GO  TO  30 
IF  (C2DOT.GT.X2DOT)  SW2-1 
IF  (SW2.EQ.1)  GO  TO  40 
KS2«ABS(2.*CR2)/(((N2*T)**2)*V2) 
KK2-KS2 


V 
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CONTINUE 

C2=CR2 

C2  DOT=CR2  DOT 

N2=N2+1 

CONTINUE 

IF  (N3.EQ.0.0)  GO  TO  50 

IF  ( C3 DOT . GT . X3 DOT )  SW3=1 

IF  (SW3.EQ.1)  GO  TO  60 

KS3=ABS ( 2 . *CR3 ) / ( ( (N3*T) **2) *V3) 

KM3=KS3 

CONTINUE 

C3=CR3 

C3  DOT=CR3  DOT 

N3=N3+1 

CONTINUE 


SORT 


■ft*********************************************************** 

TERMINAL 
METHOD  RKSFX 

CONTRL  FINTIM=0 . 08 , DELT=0 . 00005 , DELS=0 . 00025 

SAVE  (SI)  0 . 0001 , XXDOT , XYDOT , XZDOT , CXDOT , CYDOT , CZDOT , . . . 

CRXDOT , CRYDOT , CRZ  DOT , CRX , CRY , CRZ 
SAVE  (S2)  0.001, CX, CRX, RX, CY, CRY,RY, CZ , CRZ , RZ 
GRAPH  (Gl/Sl  ,  DE  =  TEK618  ,  PO  =  l) 
CRX (LE=6.0,NI=11,UN=« INCHES’ ) . . . 

CXDOT (NI=3,LO=-30,SC=30, UN* ' INCHES/SEC ' ) , . . . 

CRXDOT (NI=3 , LO=-3  0 , PO=6 . 0 , SC=30 , UN= ' INCHES/SEC ' )  , . . . 
XXDOT (NI=3 , LO=-3 0 , SC=3  0 , AX=OMIT ) 

GRAPH  (G2/S1 , DE=TEK618 , OV, PO=l ,3.25) . . . 

CRY (LE=6 . 0, NI-11 ,UN= ' INCHES  ' ) ,  ... 

CYDOT (NI=3 , LO=-3 5 , SC=35 , UN= ' INCHES/SEC '),... 

CRYDOT (NI=3,LO=-3 5, PO=6.0,SC=3 5, UN= ' INCHES/SEC ' ) , . . . 
XYDOT (NI=3 , LO=-3  5 , SC=3  5 , AX=OMIT) 

GRAPH  (G3/S1 , DE=TEK618 , OV, PO=l , 6.5) . . . 

CRZ (LE=6.0,NI=11,UN=' INCHES' ) , . . . 
CZDOT(NI=3,LO=-30,SC=30,UN=' INCHES/SEC' ) , . . . 

CRZ DOT (NI=3 , LO=-3  0 , PO=6 . 0 , SC=3  0 , UN= ' INCHES/SEC ' ) , . . . 
XZDOT (NI=3 , LO=-3  0 , SC=3  0 , AX=OMIT) 

GRAPH  (G4/S2 , DE=TEK618 , PO=l)  TIME (LE=6 . 0 , NI=8 ,UN= ' SEC '),.. . 
CX (NI=3 , LO=0 . 0, UN= ' INCHES ' , SC=0 . 5 , PO=6 . 0) , . . . 

CRX (NI=3,LO=0.0,UN=' INCHES' ,SC=0.5) , . . . 

RX (NI=3 , LO=0 . 0 , SC=0 . 5 , AX=OMIT) 

GRAPH  (G5/S2 , DE=TEK618 , OV, PO=l ,3.25) . . . 

TIME (LE=6.0,NI=8,UN=' SEC ' ) , . . . 

CY(NI=3,LO=0.0,UN=' INCHES' , SC=0 . 5 , PO=6 . 0 ) , . . . 

CRY (NI=3,LO=0.0,UN=' INCHES' ,SC=0.5) , . . . 

RY (NI=3 , LO=0 . 0 , SC=0 . 5 , AX=OMIT) 

GRAPH  (G6/S2 , DE=TEK618 , OV, PO=l ,6.5) . . . 

TIME (LE=6 . 0 , NI=8 , UN= ' SEC ' ) , . . . 


CZ (NI=3,LO=0.0,UN=' INCHES » ,sc=0. 5, PO=6.0) , . . . 
CRZ (NI=3 , LO=0 . 0 , UN= ' INCHES ' , SC=0 . 5 ) , . . . 

RZ (NI=3 , L0=0 . 0 , SC=0 . 5 , AX=OMIT) 

LABEL  (Gl)  PHASE  PLANE  (CDOT, CRDOT, XDOT. VS . CR) 

LABEL  (G4)  STEP  RESPONSE 

END 

STOP 


************************************************************ 
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DERIVATION  OF  MATHEMATICAL  MODEL  FOR  THE  THREE  LINK 

REVOLUTE  ROBOT  ARM 


First  the  kinetic  energy  of  each  link  must  be  computed 


Ki  =  l/2miUi2 


(G.l) 


and  then  the  potential  energy,  which  is  related  to  the 
vertical  height  of  the  mass  expressed  by  the  z  coordinate, 
must  be  calculated  as 


Vi  =  migzi 


(G.2) 


It  is  obvious  that  the  kinetic  energy  of  mass  mx  is 


Kx  =  0 


(G.3) 


and  the  potential  energy 


Vx  =  migdi 


(G.4) 


In  the  case  of  masses  m2  and  m3  the  expressions  for  the 
velocity  squared  of  the  masses  must  be  obtained  in  order  to 
compute  the  kinetic  energy.  Thus,  we  will  first  write  the 
expressions  for  the  Cartesian  position  coordinates  and  then 
differentiate  them  to  obtain  the  velocity. 

For  the  mass  m2  the  Cartesian  position  coordinates  are 


WWW! 


(G.5) 


x2  =  d2cos02cos®l 

y2  =  d2cos©2sin01  ( G -  6 ) 

z2  ~  d1+d2sin©2  (G.7) 

and  the  Cartesian  components  of  the  velocity  are  then 

x2  *  -d2sin©2cos©1©2-d2COs©2sin©i©i  (G.8) 

y2  =  -d2sin©2sin9i02+d2cos©2cos©1©l  (G.9) 

z2  =  d2COS0292  (G.10) 

The  magnitude  of  the  velocity  squared  is  then 
u22  =  X22+Y22+z22 

=  d22 (cos202©12+022)  (G. 11) 

and  the  kinetic  energy  is 

K2  =  l/2m2d22 (cos2©2©i2+e22)  (G . 12 ) 

The  height  of  the  mass  m2  is  expressed  by  equation  G.7,  a: 
the  potential  energy  is  then 

V2  =  m2g(d1+d2sin©2)  (G.13) 

For  the  mass  m3  the  Cartesian  position  coordinates  are 

x3  =  [d2cos02+d3cos (©2+63) ] cos©i  (G.14) 

y3  =  [d2cos©2+d3cos (©2+©3 ) ] sin©^  (G.15) 

z3  =  d3+d2sin©2+d3sin (©2+63 )  (G.16) 

and  the  Cartesian  components  of  the  velocity  are  then 
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(G. 17) 


X3  =  -[d2COse2+d3COS(02+©3) Jsin©!©! 

-[d2Sine202+d3sin(®2+®3) (®2+®3) ]cOs®i 
Y3  ~  [d2cose2+d3COS (62+83) 3COS0!©!  (G.18) 

-[d2sin0202+d3sin(02+83)  (©2+83)  ]sin©]_ 

Z3  =  d2COS©202+d3COS (©2+63) (82+83)  (G.19) 

The  magnitude  of  the  velocity  squared  is  then 
u32  =  x32+y32+z32 

=  d22 [cos2©2812+©22]  (G . 20 ) 

+2d2d3 [cos©2cos (©2+83 ) ©i2+cos©3©2 (©2+83 ) ] 

+d32 [cos2 (©2+83) 0i2+022+28203+832 ] 

and  the  kinetic  energy  is 

K3  =  l/2m3d22 [cos2©2812+822 ] 

+m3d2d3 [cos©2cos (82+83) 8^2+cos©302 (©2+83) ]  (G.21) 

+l/2m3d32 [cos2 (02+83)812+©22+20283+©32] 

The  height  of  the  mass  ra3  is  expressed  by  equation  G.16  and 
the  potential  energy  is  then 

V3  =  m3g[d1+d2sine2+d3sin(©2+©3) ]  (G.22) 

The  Langrangian,  L  =  Kj.  -  V^,  is  then  obtained  from 
equations  G.3,  G.4,  G.12,  G.13,  G.21,  and  G.22 

L  =  [ 1/2 (m2+m3 ) d22cos2e2+m3d2d3COS©2COs (©2+83 ) 

+l/2m3d32cos2  (©2+83)+l/2Jinl]©12  (G.23) 

+[1/2 (m2+m3 ) d22+m3d2d3cos03+l/2 (m3d32 ) +1/2 Jm2 ] ©2  2 


+  [1/2  (m3d3i:+Jia3)  ]©3^ 

+m3 (d32+d2d3cos03) 02®3 

-[  (013+1112+1113  )di-(m2+m3)d2s^ne2_1,l3d3s^n(02+e3)  3  9 

Taking  partial  derivatives  of  equation  G.23  with  respect  to 
03,  ©2  and  ©3 

01* 

-  -  0  (G. 24) 

0®1 

[  (012+013)  d2 2 sin©2 cos02+m3d2d3 sin  (202+03) 

+m3d32sin(02+©3) cos (©3+03) ]©32  (G. 25) 

(012+013 )  gd2cos©2-oi3gd3cos  (02+03 ) 

0L 

-  =  - [m3d2d3Sin (02+03 ) cos02 

003 

+m3d32sin(02+03) cos(02+03) ]©32 

-m3d2d3sin03022  (G.26) 

•  • 

-m3d2d3sin030203 

-oi3gd3cos  (02+®3 ) 

Taking  partial  derivatives  of  equation  G.23  with  respect  to 
©1,  02  and  03 

[  (012+103)  d22cos202+2oi3d2d3cos02cos  (02+03 )  (G.27) 

+m3d32cos2  (02+03)+Jjjj3]03 

[  (013+013)  d22+2oi3d2d3COS03+in3d32+Jin2  ]®2 

+  [m3d2d3COS03+m3d32  303 


QL 

0el 


01* 

0®2 


01* 

002 


(G. 28) 


3l 

S©3 


[3n3d32+Jm3]03+[m3d2d3COs03+m3d32]02 


Taking  derivatives  of  equations  G.27,  G.28,  and 
respect  to  time 


d/3L  \ 

—  — : —  83  Cm3  (d2cos02+d3cos (02+03) )  z 
dt  \3@i/ 

+®2d22cos202+Jmi3®i 
-2 [ (m2+m3 ) d22sin©2cos02 
+m3d2d3sin( 282+63) 
+m3d32sin(02+03) cos (©2+63) ]©i©2 
-2 [m3d2d3sin(©2+03) cos©2 

+m3d32sin(©2+03) cos (©2+03) ]©i©3 


d/3L  \ 

dt  \0o2  y 


d/3L 


dt  \3©3 


=  [ (m2+m3)d22+2m3d2d3cos03+m3d32+Jm2]e2 
+ [m3d2d3COS03+m3d32 ] 03 
-2m3d2d3sin030203 
-m3d2d3sine3032 

=  [m3d32+Jm3 ]©3 
+[m3d2d3cos83+m3d32 ]92 
-m3d2d3sin03e293 


The  corresponding  torques  are  given  by 


d  /  3L 


3L 


Ti  -  — 


dt\c)0i/  3®i 


for  i=l , 2 , 3 


(G. 29) 

.29  with 

(G. 30) 

(G. 31) 

(G . 32 ) 

(G. 33) 
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Substituting  equations  G.24  -  G.32  into  equation  G.33 


T1  =  [m3 (d2COse2+d3COs(e2+93) ) 2 

+m2d22cos2e2+Jml3®l 

-2  [  (m2+n»3)  d2 2 sin02cos82+m3d2d3 sin  (262+63) 

+m3d32sin(02+Q3) cos (62+63) ]6i62  (G. 34) 

-2 [m3d2d3sin (62+63 ) cos©2 

+m3d3^sin (©2+63) cos (©2+63) ]9i03 

T2  =  [  (m2+ni3)d22+2iii3d2d3cos03+m3d32+Jin2  362 
+  [xa3d2d3cos03+m3d32  ]©3 

-2m3d2d3sin©3©203-m3d2d3sin03632  (G. 35) 

+  ( (ni2+ni3 )  d22sin©2COS©2+in3d2d3sin  ( 262+63 ) 

+m3d32sin (©2+63) cos (©2+63) ]6i2 
+ (m2+m3 ) gd2Cos©2+m3gd3cos (©2+63 ) 

T3  =  [m3d32+Jm3]©3+[ia3d2d3cos©3+in3d32]©2 

+[m3d2d3sin(02+©3) cos©2  (G.36) 

+m3d32sin(82+©3) cos (02+©3) ]®i2 

+m3d2d3sin©3©22+m3gd3cos(©2+03) 
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DSL  PROGRAM  FOR  THE  THREE  LINK  REVOLUTE  ROBOT  UNDER 
DIFFERENT  LOAD  CONDITIONS  (NO  GRAVITY) 


TITLE  MODEL  OF  REVOLUTE  3 -DEG  OF  FREEDOM  ROBOT  ARM 
PARAM  K**l.  0 ,  K1=0 . 6 ,  K2=10000 . 0  ,  KM1=0 . 4225  ,  KM2=0 . 4225 ,  KM3=4 . 0 
PARAM  VSAT=150 . 0 ,M1=0 .268 ,M2=0 .221 , M3=0 .041 
PARAM  D1=15.0,D2=10.0,D3=10.0,G=386.4 

PARAM  11=0 . 033 , J2=0 .033 , J3=0 . 033 , R1=0 . 91 , R2=0 .  91 , R3=0 . 91 
PARAM  KT1=14 . 4 ,KT2=14 . 4 , KT3=14 . 4 , Ll=. 0001 , L2-. 0001 , L3=. 0001 
PARAM  BM1=0 . 04297 , BM2=0 . 04297 , BM3=0 . 04297 , LOAD=0 . 0 
PARAM  KV1=0. 1012 ,KV2=0. 1012, KV3=0. 1012, T=0. 00025 
PARAM  REF1=1 . 0  ,REF2=1.0  ,REF3=1. 0 
INTGER  SW1 , SW2 , SW3 , N1 , N2 , N3 

*  Kl:  THE  CURVE  SCALING  CONSTANT 

*  K2:  THE  AMPLIFIER  GAIN 

*  KM:  THE  INITIAL  CONSTANT  OF  THE  IDEAL  (MODEL)  MOTOR 

*  VSAT:  THE  SATURATION  LIMITS  OF  THE  AMPLIFIER 

*  K:  THE  VELOCITY  LOOP  FEEDBACK  GAIN  (OF  THE  MODEL) 

*  REF1 , REF2 , REF3 : THE  COMMANDED  TIP  POSITION  IN  RADS 

*  T:  THE  SAMPLING  INTERVAL 
INITIAL 

FLAG1=0 

FLAG2-0 

FLAG3=0 

N1=0 

N2=0 

N3=0 

X1DOT=0 . 

X2DOT=0. 

X3DOT=0. 

C1=0. 

C2=0 . 

C3=0 . 

C1DT=0. 

C2DT=0. 

C3DT=0. 

C1DDT=0. 

C2DDT=0 . 

C3DDT=0. 

CR1=0. 

CR2=0. 

CR3=0. 

CR1DT=0. 

CR2DT=0 . 

CR3DT=0. 

CR1DDT=0. 


CR2DDT=0. 

CR3DDT=0. 

TL1=0 . 

TL2=0 . 

TL3=0 . 

MP1=0 . 

MP2=0. 

MP3=0 . 

MT1=0 . 

MT2=0. 

MT3=0. 

A1=SQRT(2.*KM1*VSAT) 
A2=SQRT ( 2 . *KM2*VSAT) 
A3=SQRT ( 2 . *KM3 *VSAT) 
DERIVATIVE 

RR1=REF1*STEP (0.0) 
RR2=REF2*STEP (0.0) 
RR3=REF3*STEP (0.0) 
E1=RR1-C1 
E2=RR2-C2 
E3=RR3-C3 


NOSORT 


Dll  =  M3* (D2*C0S (CR2 ) +D3*C0S (CR2+CR3 ) ) **2 . . . 

+M2*D2**2* (COS (CR2 ) ) **2 
D112=  2* ( (M2+M3) *D2**2*COS (CR2) *SIN (CR2) . . . 

+M3*D3**2*COS (CR2+CR3 ) *SIN (CR2+CR3 ) . . . 
+M3*D2*D3*SIN (2*CR2+CR3) ) 

D113*  2* (M3*D3**2*COS (CR2+CR3 ) *SIN (CR2+CR3 ) . . . 

+M3*D2*D3*COS (CR2 ) *SIN (CR2+CR3 ) ) 

D22  =  (M2+M3) *D2**2+M3*D3 **2+2*M3*D2*D3*SIN (CR3 ) 
D23  -  M3*D3**2+M3*D2*D3 *SIN (CR3 ) 

D211=  (M2+M3) *D2**2*COS (CR2) *SIN (CR2) . . . 

+M3*D3**2*COS (CR2+CR3) *SIN (CR2+CR3) . . . 
+M3*D2*D3*SIN(2*CR2+CR3) 

D223=  2*M3*D2*D3*SIN (CR3 ) 

D233=  M3*D2*D3*SIN(CR3) 

D32  =  M3*D3**2+M3*D2*D3*COS (CR3) 

D33  =  M3*D3**2 

D311=  M3*D3**2*COS (CR2+CR3 ) *SIN (CR2+CR3) . . . 

+M3*D2*D3*COS (CR2) *SIN (CR2+CR3 ) 

D322=  M3*D2*D3*SIN (CR3) 

G2  =  (M2+M3)  *G*D2*C0S  (CR2)  +M3*G*D3*COS  (CR2+CR3) 
G3  =  M3*G*D3*COS (CR2+CR3 ) 

TL1  =  -D112*CR1DT*CR2DT-D113*CR1DT*CR3DT 
TL2  =  D23*CR3DDT+D211*CR1DT**2-D233*CR3DT**2. . . 
-D223*CR2DT*CR3DT 

TL3  =  D32*CR2DDT+D311*CR1DT**2+D322*CR2DT**2 
JTOT1  =  Dll+Jl 
JT0T2  =  D22+J2 
JT0T3  =  D33+J3 

IF (El. LT. 0.0)  X1D0T=-A1*K1*SQRT ( ABS (El) ) 


IF(El.GE.O.O)  X1D0T=A1*K1*SQRT(E1) 

IF (E2 . LT. 0 . 0)  X2DOT=-A2*Kl*SQRT(ABS(E2) ) 

IF (E2 . GE .0.0)  X2D0T=A2*K1*SQRT(E2) 

IF(E3.LT. 0.0)  X3DOT=-A3*Kl*SQRT(ABS(E3) ) 

IF (E3 . GE .0.0)  X3D0T=A3*K1*SQRT(E3) 

SORT 

************************************************************ 

KC1D0T=K*C1DT 

X1D0TE=X1D0T-KC1D0T 

V1=LIMIT ( -VSAT , VS AT , K2  *X1D0TE ) 

C1DDT=V1*KM1 

NOSORT 

IF  (FLAG1.EQ.1)  GO  TO  2 

IF  (VI. LT. VSAT. AND. TIME. GT. 0.00005)  FLAG1=1 
NSW1=N1 

2  CONTINUE 

SORT 

C1DT=INTGRL ( 0 . 0 , C1DDT) 

C1=INTGRL ( 0 . 0 , C1DT) 

VM1=V1-KV1*CR1DT 
MP1=REALPL (0.0, Ll/Rl , VM1/R1 ) 

MT1— KT1*MP1 

MT1E=MT1-BM1*CR1DT-TL1 
CR1DDT= ( 1 . / JTOT1 ) *MTIE 
CR1DT=INTGRL ( 0 . 0 , CR1DDT) 

CR1=»INTGRL  (0.0,  CR1DT ) 

************************************************************ 

KC2DOT=K*C2DT 

X2 DOTE=X2 DOT-KC2 DOT 

V2=LIMIT ( -VSAT , VSAT , K2  *  X2  DOTE ) 

NOSORT 

IF  (FLAG2.EQ.1)  GO  TO  4 

IF  (V2.LT. VSAT. AND. TIME. GT. 0.00005)  FLAG2=1 
NSW2=N2 

4  CONTINUE 

SORT 

C2DDT=V2*KM2 
C2 DT=INTGRL ( 0 . 0 , C2  DDT ) 

C2=INTGRL (0.0, C2DT) 

VM2=V2 -KV2  *CR2  DT 
MP2=REALPL (0.0, L2/R2 , VM2/R2 ) 

MT2=KT2  *MP2 

MT2E=MT2-BM2*CR2DT-TL2 
CR2DDT= ( 1 ./JTOT2 ) *MT2E 
CR2 DT=INTGRL (0.0, CR2 DDT) 

CR2=INTGRL (0.0, CR2DT) 


KC3DOT=K*C3DT 

X3 D0TE-X3  D0T-KC3  DOT 

V3=LIMIT ( -VS AT , VSAT , K2  *X3  DOTE ) 

NOSORT 

IF  (FLAG3.EQ.1)  GO  TO  6 

IF  (V3.LT. VSAT. AND. TIME. GT. 0.00005)  FLAG3=1 
NSW3=N3 

6  CONTINUE 

SORT 

C3DDT=V3  *KM3 
C3 DT=INTGRL ( 0 . 0 , C3  DDT ) 

C3=INTGRL ( 0 . 0 , C3 DT ) 

VM3=V3 -KV3 *CR3 DT 
MP3=REALPL (0.0, L3/R3 , VM3/R3 ) 

MT3=KT3*MP3 

MT3E=MT3-BM3*CR3DT-TL3 
CR3DDT=(l./JTOT3) *MT3E 
CR3DT=INTGRL(0. 0, CR3DDT) 

CR3=INTGRL (0.0, CR3 DT) 

A*********************'*******'**********************'*******'** 

SAMPLE 

NOSORT 

IF  (N3.EQ.0)  GO  TO  30 

IF  (N2.EQ.0)  GO  TO  20 

IF  (N3.EQ.0)  GO  TO  10 

C3=CR3 

C2=CR2 

C1=CR1 

KS3=ABS (2 . *CR3) / ( ( (N3*T) **2) *V3) 

KS2=ABS ( 2 . *CR2 ) / ( ( (N2  *T) *  *2 ) *V2 ) 

KS1=ABS (2 . *CR1) / ( ( (N1*T) **2) *V1) 

IF  (FLAG3.EQ.0)  KM3=KS3 
IF  (FLAG2.EQ.0)  KM2=KS2 
IF  (FLAG1.EQ.0)  KM1=KS1 
IF  (N3.GE.2)  CR3DTL= (CR3-CR32L) / (2 . *T) 

IF  (N2.GE.2)  CR2DTL= ( CR2-CR2  2L) / ( 2 . *T) 

IF  (N1.GE.2)  CR1DTL=(CR1-CR12L)/(2.*T) 

IF (FLAG3 . EQ. 0)  C3DT=(2.*( (CR3-CR3LST) /T) )-CR3DTL 
IF(FLAG2 . EQ. 0)  C2DT= ( 2 . * ( (CR2-CR2LST) /T) )-CR2DTL 
IF (FLAG1 . EQ. 0)  C1DT= (2 . * ( (CR1-CR1LST) /T) ) -CR1DTL 
IF  (N3 . EQ. NSW3 .AND. FLAG3 . EQ. 1)  GO  TO  30 
IF  (N2 .EQ.NSW2 .AND.FLAG2 .EQ. 1)  GO  TO  20 
IF  (N1 . EQ . NSW1 . AND. FLAG1 . EQ . 1)  GO  TO  10 
IF  (FLAG3.EQ.1)  C3DT= (2 . * ( (CR3-CR3LST) /T) ) -CR3DTL 
IF  (FLAG2.EQ.1)  C2DT= ( 2 . * ( (CR2-CR2LST) /T) ) -CR2DTL 
IF  (FLAG1.EQ.1)  C1DT= (2 . * ( (CR1-CR1LST) /T) ) -CR1DTL 
30  N3=N3+1 


N2-N2+1 

Nl-Nl+1 

CR3DTL=C3DT 

CR2DTL=C2DT 

CR1DTL=C1DT 

CR3  2  L—CR3  LST 

CR2 2  L=CR2 LST 

CR12  L=CR1LST 

CR3LST=CR3 

CR2LST=CR2 

CR1LST=CR1 


SORT 


************************************************************* 

TERMINAL 
METHOD  RKSFX 

CONTRL  FINTIM=0. 5, DELT=0 . 00005, DELS=0. 00025 

SAVE  (SI)  0 . 0004 , X1DOT, X2DOT, X3DOT, C1DT, C2DT, C3DT, . . . 

CR1DT , CR2 DT , CR3 DT, CR1, CR2 , CR3 
SAVE  (S2)  0.001, Cl, CR1,RR1,C2,CR2,RR2,C3,CR3,RR3 
SAVE  (S3)  0.001, El, E2,E3 
PRINT  0.01,  E1,E2,E3 
GRAPH  (Gl/Sl , DE=TEK618 , PO=l) . . . 

CR1 (LE=6 . 0 , NI=13 , LO=- . 1 , SC= . 1 , UN= ' RAD ' ) , . . . 
C1DT(LE=3,NI=4,L0=-3,SC=3,UN=' RAD/SEC') , . . . 

CR1DT ( LE=3 , NI=4 , LO=-3 , PO=6 . 0 , SC=3 , UN= ' RAD/SEC 
XI DOT ( LE=3 , NI=4 , LO=-3 , SC=3 , AX=OMIT) 

GRAPH  (G2/S1,  DE=TEK618  ,OV,  PO=l,  3.25)  .  .  . 

CR2 (LE=6 . 0 , NI=13 , LO=- . 1 , SC= . 1 , UN= 1  RAD ' ) , . . . 

C2DT (LE=3 , NI=4 , LO=-3 , SC=3 , UN= ' RAD/ SEC ' ) , . . . 

CR2DT (LE=3 , NI=4 , LO=-3 , PO=6 . 0 , SC=3 , UN= 1  RAD/SEC ' )  , . . . 
X2DOT (LE=3 ,NI=4 , LO=-3 , SC=3 , AX=OMIT) 

GRAPH  (G3/S1 , DE— TEK618 , OV, PO=l ,6.5) . . . 

CR3 ( LE=6 . 0 , NI=13 , LO=- . 1 , SC= . 1 , UN= ' RAD ' ) , . . . 

C3DT (LE=3,NI=4,LO=-8,SC=8,UN=' RAD/SEC ' ) , . . . 

CR3DT ( LE=3 , NI=4 , LO=-8 , PO=6 . 0 , SC=8 , UN= ' RAD/SEC ' ) , . . . 
X3DOT (LE=3 , NI=4 , LO=-8 , SC=8 , AX=OMIT) 

GRAPH  (G4/S2 , DE=TEK618 , PO=l)  TIME (LE=6 . 0 , NI=5 , UN= ' SEC *)/•• • 
Cl ( LE=3 , NI=4 , LO=- . 2 , UN= ' RAD ' , SC=0 . 4 , PO=6 . 0) , . . . 

CR1 (LE=3 , NI=4 , LO=- . 2 , UN= • RAD ' , SC-0. 4) , . . . 

RR1 ( LE=3 , NI=4 , LO=- . 2 , SC=0 . 4 , AX=OMIT) 

GRAPH  (G5/S2 , DE=TEK618 , OV, PO=l ,3.25) . . . 

TIME (LE=6.0,NI=5,UN=' SEC) , . . . 

C2 ( LE=3 , NI=4 , LO=- . 2 , UN= ' RAD ' , SC=0 . 4 , PO=6 . 0) , . . . 

CR2 ( LE=3 , NI=4 , LO=- . 2 , UN= ' RAD ' , SC-0. 4) , . . . 

RR2  ( LE=3  ,  NI=4  ,  LO—  .  2  ,  SC=0 . 4  ,  AX=OMIT) 

GRAPH  (G6/S2 , DE=TEK618 , OV, PO=l ,6.5) . . . 

TIME ( LE=6 . 0 , NI=5 , UN= ' SEC '),... 

C3 ( LE=3 , NI=4 , LO=- . 2 , UN= ' RAD ' , SC=0 . 4 , PO=6 . 0) , . . . 

CR3 ( LE=3 , NI=4 , LO=- . 2 , UN= ' RAD ' ,SC=0. 4) , . . . 


RR3 ( LE=3 , NI=4 , LO=- . 2 , SC=0 . 4 , AX=OMIT) 

GRAPH  (G7/S3,DE=TEK618,PO=l, .5) . . . 

TIME (LE=5.0,NI»5,UN=' SEC') , . . . 

El ( LE=8 , NI=7 , L0=- . 2 , UN= ' RAD ' , SC=. 2) , . . . 

E2 ( LE=8 , NI=7 , L0=- . 2 , UN= ' RAD ' , SC= . 2 , P0=5 . 0 ) , . . . 

E3 ( LE=8 , NI=7 , L0=- . 2 , UN= ' RAD ' , SC= . 2 , P0=6 . 2 ) 
LABEL  (Gl)  PHASE  PLANE  ( CDT , CRDT , XDOT . VS . CR) 

LABEL  (G4)  STEP  RESPONSE 
LABEL  (G7)  ERROR  .VS.  TIME 
END 
STOP 


************************************************************ 


DSL  PROGRAM  FOR  THE  THREE  LINK  REVOLOTE  ROBOT 
WITH  DISTURBANCE 


TITLE  MODEL  OF  REVOLUTE  3 -DEG  OF  FREEDOM  ROBOT  ARM 
PARAM  K“l. 0 , K1=0 . 6, K2=10000 . 0 , KM1=0 . 4225, KM2=0 . 4225 , KM3=4 . 0 
PARAM  VSAT=150.0,M1=0.268,M2=0.227 ,M3=0.041 
PARAM  Dl=15 . 0 , D2=10 . 0 , D3=10 . 0 , G=386 . 4 

PARAM  Jl=0.033, J2=0.033, J3=0 . 033 , R1=0 . 91 , R2=0 . 91 , R3=0 . 91 
PARAM  KT1=14.4,KT2*=14.4,KT3=14.4,L1=.0001,L2=.0001,L3=.0001 
PARAM  BM1=0. 04297, BM2=0. 04297, BM3=0. 04297, LOAD=0.0 
PARAM  KV1=0 . 1012 , KV2=0 . 1012 , KV3=0 . 1012 , T=0 .00025 
PARAM  REF 1=1 . 0  ,REF2=1.0  ,REF3=1.0 
INTGER  SW1 , SW2 , SW3 , N1 , N2 , N3 

*  Kl:  THE  CURVE  SCALLING  CONSTANT 

*  K2:  THE  AMPLIFIER  GAIN 

*  KM:  THE  INITIAL  CONSTANT  OF  THE  IDEAL  (MODEL)  MOTOR 

*  VSAT:  THE  SATURATION  LIMITS  OF  THE  AMPLIFIER 

*  K:  THE  VELOCITY  LOOP  FEEDBACK  GAIN  (OF  THE  MODEL) 

*  REF1 , REF2 , REF3 : THE  COMMANDED  TIP  POSITION  IN  RADS 

*  T:  THE  SAMPLING  INTERVAL 
INITIAL 

FLAG1=0 

FLAG2=0 

FLAG3=0 

N1=0 

N2=0 

N3=0 

X1DOT=0 . 

X2DOT=0. 

X3DOT=0. 

C1=0. 

C2=0. 

C3=0. 

C1DT=0. 

C2DT=0. 

C3DT=0. 

C1DDT=0 . 

C2DDT=0. 

C3DDT=0 . 

CR1=0. 

CR2—0 . 

CR3=0. 

CR1DT=0 . 

CR2DT=0 . 

CR3DT=0. 

CR1DDT=0 . 


I 


CR2DDT=0 . 

CR3DDT=0. 

TL1=0 . 

TL2=0. 

TL3=0. 

MP1=0. 

MP2=0 . 

MP3=0. 

MT1=0. 

MT2=0. 

MT3=0. 

A1=SQRT (2 . *KM1*VSAT) 

A2»SQRT ( 2 . *KM2  *VSAT) 

A3=SQRT ( 2 . *KM3  *VSAT) 

DERIVATIVE 

RR1=REF1*STEP (0.0)+50* (STEP ( . 090) -STEP( . 100) ) 

RR2=REF2*STEP ( 0 . 0) +50* (STEP( . 090) -STEP( . 100) ) 

RR3=REF3*STEP (0 . 0 ) +50* (STEP ( . 090) -STEP( . 100) ) 

E1=RR1-C1 

E2=RR2-C2 

E3=RR3-C3 

NOSORT 

Dll  =  M3* (D2*COS (CR2 ) +D3*COS (CR2+CR3 ) ) **2 . . . 

+M2*D2**2* (COS (CR2) ) **2 
D112=  2* ( (M2+M3 ) *D2**2*COS (CR2 ) *SIN (CR2 ) . . . 

+M3*D3**2*COS (CR2+CR3 ) *SIN (CR2+CR3 ) . . . 
+M3*D2*D3*SIN(2*CR2+CR3) ) 

D113®  2* (M3*D3**2*COS (CR2+CR3 ) *SIN (CR2+CR3 ) . . . 

+M3  *D2  *D3  *COS ( CR2 ) *SIN ( CR2+CR3 ) ) 

D22  =  (M2+M3) *D2**2+M3*D3**2+2*M3*D2*D3*SIN (CR3) 
D23  =  M3*D3**2+M3*D2*D3*SIN(CR3) 

D211=  (M2+M3) *D2**2*COS(CR2) *SIN(CR2) . . . 

+M3*D3**2*COS (CR2+CR3 ) *SIN (CR2+CR3 ) . . . 

+M3  *D2 *D3 *SIN ( 2  *CR2+CR3 ) 

D223=  2*M3*D2*D3*SIN (CR3 ) 

D233=  M3*D2*D3*SIN (CR3 ) 

D32  =  M3*D3**2+M3*D2*D3*COS (CR3) 

D33  =  M3*D3**2 

D311=  M3*D3**2*COS (CR2+CR3 ) *SIN (CR2+CR3 ) ... 

+M3*D2*D3*COS (CR2) *SIN (CR2+CR3) 

D322=  M3*D2*D3*SIN (CR3) 

G2  =  (M2+M3) *G*D2*COS (CR2 ) +M3*G*D3*COS (CR2+CR3) 
G3  =  M3*G*D3*COS (CR2+CR3 ) 

TL1  =  -D112*CR1DT*CR2DT-D113*CR1DT*CR3DT 
TL2  =  D23*CR3DDT+D211*CR1DT**2-D233*CR3DT**2 . . . 
-D2 2  3  *CR2DT*CR3 DT 

TL3  =  D32  *CR2DDT+D3 11*CR1DT**2+D322  *CR2DT**2 
JTOT1  =  Dll+Jl 
JTOT2  =  D22+J2 
JTOT3  =  D33+J3 

IF (El. LT. 0.0)  X1D0T=-A1*K1*SQRT (ABS (El) ) 
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IF (El . GE . 0 . 0)  XlDOT=Al*Kl*SQRT (El) 

IF (E2 . LT .0.0)  X2D0T=-A2*K1*SQRT (ABS (E2) ) 

IF (E2 . GE . 0 . 0)  X2D0T=A2*K1*SQRT (E2 ) 

IF (E3 . LT. 0 . 0)  X3DOT=-A3  *K1*SQRT (ABS (E3 ) ) 

IF (E3 . GE . 0 . 0)  X3D0T=A3*K1*SQRT (E3 ) 

SORT 

************************************************************ 

KC1D0T=K*C1DT 

XlDOTE=XlDOT-KClDOT 

V1=LIMIT ( -VSAT , VS AT , K2  *X1D0TE) 

C1DDT=V1*KM1 

NOSORT 

IF  (FLAG1.EQ. 1)  GO  TO  2 

IF  ( VI. LT. VSAT. AND. TIME. GT. 0.00005)  FLAG1=1 
NSW1=N1 

2  CONTINUE 

SORT 

C 1 DT= I NTGRL (0.0, C1DDT) 

C1=INTGRL ( 0 . 0 , C1DT) 

VM1=V1-KV1*CR1DT 
MP1=REALPL (0.0, Ll/Rl , VM1/R1 ) 

MT1=KT1*MP1 

MT1E=MT1-BM1*CR1DT-TL1 
CR1DDT= ( 1 . / JTOT1 ) *MT1E 
CR1 DT= INTGRL (0.0, CR1DDT) 

CR1=INTGRL (0.0, CR1DT) 

************************************************************ 

KC2DOT=K*C2DT 

X2 DOTE=X2  DOT-KC2 DOT 

V2=LIMIT ( -VSAT , VSAT , K2  *X2DOTE ) 

NOSORT 

IF  (FLAG2.EQ.1)  GO  TO  4 

IF  (V2.LT. VSAT. AND. TIME. GT. 0.00005)  FLAG2=1 
NSW2=N2 

4  CONTINUE 

SORT 

C2DDT=V2*KM2 
C2 DT«INTGRL (0.0, C2  DDT ) 

C2=INTGRL ( 0 . 0 , C2DT) 

VM2=V2-KV2*CR2DT 
MP2=REALPL (0.0, L2/R2 , VM2/R2 ) 

MT2=KT2*MP2 

MT2E=MT2-BM2*CR2DT-TL2 
CR2DDT= ( 1 ./JTOT2 ) *MT2E 
CR2 DT= INTGRL ( 0. 0.CR2 DDT) 

CR2= INTGRL (0.0, CR2 DT) 


KC3 D0T=K*C3DT 

X3 D0TE=X3 DOT-KC 3  DOT 

V3=LIMIT ( -VSAT , VSAT , K2  *X3 DOTE ) 

NOSORT 

IF  (FLAG3.EQ.1)  GO  TO  6 

IF  (V3. LT. VSAT. AND. TIME. GT. 0.00005)  FLAG3=1 
NSW3=N3 

6  CONTINUE 

SORT 

C3DDT=V3  *KM3 
C3 DT=INTGRL (0.0,C3DDm) 

C3=INTGRL (0.0, C3DT) 

VM3=V3-KV3  *CR3DT 
MP3=REALPL (0.0, L3/R3 , VM3/R3 ) 

MT3=KT3*MP3 

MT3E=MT3-BM3*CR3DT-TL3 
CR3 DDT= ( 1 . / JTOT  3) *MT3E 
CR3 DT=INTGRL (0.0, CR3 DDT ) 

CR3=INTGRL (0.0, CR3DT) 

************************************************************ 

SAMPLE 

NOSORT 

IF  (N3.EQ.0)  GO  TO  30 

IF  (N2.EQ.0)  GO  TO  20 

IF  (N3.EQ.0)  GO  TO  10 

C3=CR3 

C2=CR2 

C1=CR1 

KS3=ABS ( 2 . *CR3 ) / ( ( (N3*T) **2) *V3 ) 

KS2=ABS (2 . *CR2 ) / ( ( (N2*T) **2) *V2) 

KS1=ABS (2 . *CR1) / ( ( (N1*T) **2) *V1) 

IF  (FLAG3.EQ.0)  KM3=KS3 
IF  (FLAG2.EQ.0)  KM2=KS2 
IF  (FLAG1.EQ. 0)  KM1=KS1 
IF  (N3.GE.2)  CR3DTL= (CR3-CR32L) / (2 . *T) 

IF  (N2.GE.2)  CR2DTL= ( CR2-CR22L) / ( 2 . *T) 

IF  (N1.GE.2)  CR1DTL= (CR1-CR12L) / (2 . *T) 

IF (FLAG3 . EQ . 0)  C3DT= (2 . * ( (CR3-CR3LST) /T) ) -CR3DTL 
IF(FLAG2.EQ.O)  C2DT= ( 2 . * ( (CR2-CR2LST) /T) ) -CR2DTL 
IF (FLAG1 .  EQ.  0)  C1DT=(2.* (  (CR1-CR1LST)  /T)  )  -CR1DTL 
IF  (N3 . EQ.NSW3 .AND. FLAG3 . EQ. 1)  GO  TO  30 
IF  (N2 . EQ . NSW2 . AND . FLAG2 . EQ . 1 )  GO  TO  20 
IF  (N1 . EQ.NSW1 .AND. FLAG 1 . EQ . 1)  GO  TO  10 
IF  (FLAG3.EQ.1)  C3DT= (2 . * ( (CR3-CR3LST) /T) ) -CR3DTL 
IF  (FLAG2.EQ.1)  C2DT= ( 2 . * ( ( CR2-CR2LST) /T) ) -CR2DTL 
IF  (FLAG1.EQ.1)  C1DT= ( 2 . * ( (CR1-CR1LST) /T) ) -CR1DTL 
30  N3=N3+1 

20  N2=N2+1 

10  N1=N1+1 


CR3DTL=C3DT 

CR2DTL=C2DT 

CR1DTL=C1DT 

CR3  2  L=CR3  LST 

CR2  2  L=CR2  LST 

CR12L=CR1LST 

CR3LST=CR3 

CR2LST=CR2 

CR1LST=CR1 


SORT 


************************************************************ 

TERMINAL 
METHOD  RKSFX 

CONTRL  FINTIM=0 . 5 , DELT=0 . 00005 , DELS=0 . 00025 

SAVE  (SI)  0 . 0004 ,XlDOT,X2DOT, X3DOT, C1DT, C2DT, C3DT, . . . 

CR1DT , CR2 DT , CR3 DT , CR1 , CR2 , CR3 
SAVE  (S2)  0.001, Cl, CR1,RR1,C2,CR2,RR2,C3,CR3,RR3 
GRAPH  (Gl/Sl , DE=TEK618 , PO=l) . . . 

CR1 ( LE=6 . 0 , NI=13 , LO=- . 1 , SC= « 1 , UN= ' RAD ' ) , . . . 

C1DT (LE=3 , NI=4 , LO=-3 , SC=3 , UN= ' RAD/SEC ' ) , . . . 

CR1DT (LE=3 , NI=4 , LO=-3 , PO=6 . 0 , SC=3 ,UN= 1  RAD/SEC  1 )  , . . . 

XI DOT ( LE=3 , NI=4 , LO=-3 , SC=3 , AX=OMIT) 

GRAPH  (G2/S1, DE=TEK618 ,0V, PO=l, 3.25) . . . 

CR2 ( LE=6 . 0 , NI=13 , LO=- . 1 , SC= . 1 , UN= ' RAD ' ) , . . . 

C2DT (LE=3 , NI=4 , LO=-3 , SC=3 ,UN= ' RAD/ SEC ' ) , . . . 

CR2DT ( LE=3 , NI=4 , LO=-3 , PO=6 . 0 , SC=3 , UN= ' RAD/ SEC ' ) , . . . 
X2DOT ( LE=3 , NI=4 , LO=-3 , SC=3 ,AX=OMIT) 

GRAPH  (G3/S1 , DE=TEK618 , OV, PO=l ,6.5) . . . 

CR3 ( LE=6 . 0 , NI=13 , LO=- . 1 , SC= . 1 , UN= ' RAD ' ) , . . . 

C3DT ( LE=3 , NI=4 , LO=-8 , SC=8 , UN= ' RAD/SEC ' ) , . . . 

CR3 DT(LE=3,NI=4,LO=-8,PO=6.0,SC=8,UN=' RAD/SEC ' ) , . . . 

X3 DOT ( LE=3 , NI=4 , LO=-8 , SC=8 , AX=OMIT) 

GRAPH  (G4/S2 , DE=TEK618 , PO=l)  TIME (LE=6 . 0 , NI=5 , UN= ' SEC ’),.. . 
Cl ( LE=3 , NI=4 , LO=- . 2 , UN= ' RAD ' , SC=0 . 4 , PO=6 . 0) , . . . 

CR1 ( LE=3 , NI=4 , LO=- . 2 , UN= ' RAD ' ,SC=0.4) , . . . 

RR1 (LE=3 , NI=4 , LO=- . 2 , SC=0 . 4 , AX=0MIT) 

GRAPH  (G5/S2 ,DE=TEK618 ,0V, PO=l, 3 . 25) . . . 

TIME ( LE=6 . 0 , NI=5 , UN= ' SEC ' ) , . . . 

C2 ( LE=3 , NI=4 , LO=- . 2 , UN= ' RAD  1 , SC=0 . 4 , PO=6 . 0)  , . . . 

CR2 ( LE=3 , NI=4 , LO=- . 2 , UN= ' RAD ' ,SC=0.4) , . . . 

RR2 (LE=3 , NI=4 , LO=- . 2 , SC=0 . 4 , AX=OMIT) 

GRAPH  (G6/S2 , DE=TEK618 , OV, PO=l ,6.5) . . . 

TIME  (LE=6.0,NI=5,TJN=' SEC  '  )  ,  .  .  . 

C3 ( LE=3 , NI=4 , LO=- . 2 , UN= ' RAD ' , SC=0 . 4 , PO=6 . 0 ) , . . . 

CR3 ( LE=3 , NI=4 , LO=- . 2 , UN= ' RAD 1 ,SC=0.4) , . . . 

RR3 ( LE=3 , NI=4 , LO=- . 2 , SC=0 . 4 , AX=OMIT) 

LABEL  (Gl)  PHASE  PLANE  ( CDT , CRDT , XDOT . VS . CR ) 

LABEL  (G4 )  STEP  RESPONSE 


I, 


p: 


DSL  PROGRAM  FOR  THE  THREE  LINK  REVOLUTE  ROBOT 
UNDER  DIFFERENT  LOAD  CONDITIONS 
(GRAVITATIONAL  TORQUES  INCLUDED) 


TITLE  MODEL  OF  REVOLUTE  3 -DEG  OF  FREEDOM  ROBOT  ARM 
PARAM  K=1 . 0, K1=0 .6,K2=10000.0, KM1=0 . 4225 , KM2=0 . 4225, KM3=4 . 0 
PARAM  VSAT=150 . 0 ,M1=0 . 268 ,M2=0 . 227 ,M3=0 . 041 
PARAM  D1=15.0,D2=10.0,D3=10.0,G=386.4 

PARAM  J1=0 .033 ,  J2=0 .033 , J3=0 .  033  , R1=0 . 91 , R2=0 . 91 , R3=0 . 91 
PARAM  KT1=14 . 4 , KT2=14 . 4 , KT3=14 . 4 , Ll=. 0001 , L2= . 0001 , L3=. 0001 
PARAM  BM1=0 . 04297 , BM2=0 . 04297 , BM3=0 . 04297 , LOAD=0 . 0 
PARAM  KV1=0. 1012, KV2=0. 1012, KV3=0. 1012, T=0. 00025 
PARAM  REF1=1 . 0  ,REF2=1.0  ,REF3=1.0 
INTGER  SW1 , SW2 , SW3 , N1 , N2 , N3 

*  Kl:  THE  CURVE  SCALLING  CONSTANT 

*  K2 :  THE  AMPLIFIER  GAIN 

*  KM:  THE  INITIAL  CONSTANT  OF  THE  IDEAL  (MODEL)  MOTOR 

*  VSAT:  THE  SATURATION  LIMITS  OF  THE  AMPLIFIER 

*  K:  THE  VELOCITY  LOOP  FEEDBACK  GAIN  (OF  THE  MODEL) 

*  REF1 , REF2 , REF3 : THE  COMMANDED  TIP  POSITION  IN  RADS 

*  T:  THE  SAMPLING  INTERVAL 
INITIAL 

FLAG1=0 

FLAG2—0 

FLAG3=0 

N1=0 

N2=0 

N3=0 

X1DOT=0 . 

X2DOT=0. 

X3DOT— 0 . 

C1=0. 

C2=0 . 

C3=0 . 

C1DT=0. 

C2DT=0. 

C3DT=0. 

C1DDT=0. 

C2DDT=0 . 

C3DDT=0 . 

CR1=0 . 

CR2=0 . 

CR3=0 . 

CR1DT=0 . 

CR2DT=0 . 

CR3  DT=0 . 


CR1DDT=0 . 

CR2DDT=0. 

CR3DDT=0 . 

TL1=0 . 

TL2=0 . 

TL3=0 . 

MP1=0. 

MP2=0. 

MP3=0 . 

MT1=0. 

MT2=0. 

MT3=0 . 

A1=SQRT (2 . *KM1*VSAT) 
A2=SQRT (2 . *KM2*VSAT) 
A3=SQRT ( 2 . *KM3 *VSAT) 
DERIVATIVE 

RR1=REF1*S  ^P(O.O) 
RR2=REF2  *STEP (O.O) 
RR3=REF3 *STEP (0.0) 
E1=RR1-C1 
E2=RR2-C2 
E3=RR3-C3 


NOSORT 


D22  = 


Dll  =  M3* (D2*COS (CR2 ) +D3*COS (CR2+CR3 ) ) **2 . . . 

+M2*D2**2* (COS (CR2) ) **2 
D112*  2* ( (M2+M3 ) *D2**2*COS (CR2 ) *SIN (CR2 ) . . . 

+M3*D3**2*COS (CR2+CR3) *SIN (CR2+CR3) . . . 
+M3*D2*D3*SIN(2*CR2+CR3) ) 

D113*  2* (M3*D3**2*COS (CR2+CR3) *SIN (CR2+CR3) ... 

+M3  *D2  *D3  *COS ( CR2 ) *SIN ( CR2+CR3 ) ) 

D22  =  (M2+M3) *D2**2+M3*D3**2+2*M3*D2*D3*SIN(CR3) 
D23  =  M3*D3**2+M3*D2*D3*SIN (CR3) 

D211=  (M2+M3 ) *D2**2*COS (CR2 ) *SIN (CR2 ) . . . 

+M3*D3**2*COS (CR2+CR3 ) *SIN (CR2+CR3 ) . . . 

+M3  *D2  *D3  *SIN ( 2 *CR2+CR3 ) 

D223=  2*M3*D2*D3*SIN (CR3 ) 

D233=  M3*D2*D3*SIN (CR3) 

D32  =  M3*D3**2+M3*D2*D3*COS (CR3 ) 

D33  =  M3*D3**2 

D3 11=  M3*D3**2  *COS ( CR2+CR3 ) *SIN (CR2+CR3 )  . . . 

+M3  *D2  *D3  *COS (CR2 ) *SIN (CR2+CR3 ) 

D322=  M3*D2*D3*SIN (CR3) 

G2  =  (M2+M3) *G*D2*COS (CR2) +M3*G*D3*COS (CR2+CR3 ) 
G3  =  M3*G*D3*COS (CR2+CR3) 

TL1  =  -D112  *CR1DT*CR2DT-D113  *CR1DT*CR3DT 
TL2  =  D23*CR3DDT+D211*CR1DT**2-D233*CR3DT**2 . . . 
-D2  2  3  *CR2  DT*CR3 DT+G2 

TL3  =  D32*CR2DDT+D311*CR1DT**2+D322*CR2DT**2+G3 
JTOT1  =  Dll+Jl 
JTOT2  =  D22+J2 
JTOT3  =  D33+J3 


I 


3 


1 


SORT 


IF (El. LT. 0.0)  X1D0T=-A1*K1*SQRT (ABS (El) ) 
IF (El . GE . 0 . 0)  X1D0T=A1*K1*SQRT (El) 

IF (E2 . LT. 0.0)  X2D0T=-A2*K1*SQRT(ABS(E2) ) 
IF  (E2 . GE .0.0)  X2D0T=A2*K1*SQRT(E2) 

IF (E3 . LT. 0.0)  X3D0T=-A3*K1*SQRT (ABS (E3) ) 
IF ( E3 . GE . 0 . 0 )  X3 DOT=A3  *K1*SQRT ( E3 ) 


************************************************************ 

KC1D0T=K*C1DT 

X1D0TE=X1D0T-KC1D0T 

V1=LIMIT ( -VSAT , VSAT , K2  *X1D0TE) 

C1DDT=V1*KM1 

NOSORT 

IF  (FLAG1.EQ.1)  GO  TO  2 

IF  ( VI. LT. VSAT. AND. TIME. GT. 0. 00005)  FLAG1=1 
NSW1— N1 

2  CONTINUE 

SORT 

C1DT=INTGRL (0.0, C1DDT) 

C1=INTGRL (0.0, C1DT) 

VM1— V1-KV1*CR1DT 
MP1=REALPL (0.0, Ll/Rl , VM1/R1 ) 

MT1=KT1*MP1 

MT1E=MT1-BM1*CR1DT-TL1 
CR1DDT= ( 1 . /JTOT1) *MT1E 
CR1DT=INTGRL (0.0, CR1DDT) 

CR1— INTGRL (0.0, CR1DT ) 

************************************************************ 


KC2DOT=K*C2DT 

X2 DOTE=X2  DOT-KC2 DOT 

V2=LIMIT ( -VSAT , VSAT , K2  *X2  DOTE ) 

IF  (FLAG2.EQ.1)  GO  TO  4 

IF  (V2. LT. VSAT. AND. TIME. GT. 0.00005)  FLAG2=1 

NSW2=N2 

CONTINUE 

C2DDT=V2*KM2 
C2 DT=INTGRL ( 0 . 0 , C2  DDT) 

C2=INTGRL( 0 . 0 , C2DT) 

VM2=V2 -KV2 *CR2  DT 
MP2=REALPL (0.0, L2/R2 , VM2/R2 ) 

MT2=KT2*MP2 

MT2 E=MT2-BM2 *CR2 DT-TL2 
CR2 DDT= ( 1 . / JTOT2 ) *MT2E 
CR2 DT= INTGRL (0.0, CR2 DDT ) 

CR2=INTGRL(0 . 0, CR2DT) 


NOSORT 


4 

SORT 


! 

\ 

w 


% 


************************************************************ 

KC3DOT=K*C3DT 

X3  D0TE=X3 DOT -KC 3 DOT 

V3=LIMIT ( -VS AT , VS AT , K2  *X3  DOTE ) 

NOSORT 

IF  (FLAG3.EQ.1)  GO  TO  6 

IF  (V3.LT.VSAT.AND.TIME.GT. 0.00005)  FLAG3=1 
NSW3=N3 

6  CONTINUE 

SORT 

C3DDT=V3*KM3 
C3  DT=INTGRL ( 0 . 0 , C3DDT) 

C3=INTGRL (0.0, C3DT) 

VM3=V3-KV3*CR3DT 
MP3=REALPL (0.0, L3/R3 , VM3/R3 ) 

MT3=KT3  *MP3 

MT3E=MT3-BM3*CR3DT-TL3 
CR3DDT= ( 1 ./JTOT3 ) *MT3E 
CR3 DT=INTGRL ( 0 . 0 , CR3  DDT ) 

CR3=INTGRL (0.0, CR3 DT ) 

************************************************************ 

SAMPLE 

NOSORT 

IF  (N3.EQ.0)  GO  TO  30 

IF  (N2.EQ.0)  GO  TO  20 

IF  (N3.EQ.0)  GO  TO  10 

C3=CR3 

C2=CR2 

C1=CR1 

KS3=ABS ( 2 . *CR3 ) / ( ( (N3*T) **2)*V3) 

•KS2=ABS  (2  .  *CR2)  /  (  (  (N2*T)  **2)  *V2) 

KS  1=ABS  ( 2  .  *CR1 )  /  (  (  (N1*T)  **2 )  *V1) 

IF  (FLAG3.EQ.0)  KM3=KS3 
IF  (FLAG2.EQ.0)  KM2=KS2 
IF  (FLAG1.EQ. 0)  KM1«KS1 
IF  (N3.GE.2)  CR3DTL= (CR3-CR32L)/ (2 . *T) 

IF  (N2.GE.2)  CR2DTL= (CR2-CR22L) / (2 . *T) 

IF  (N1.GE.2)  CR1DTL= ( CR1-CR12L) / ( 2 . *T) 

IF(FLAG3 . EQ. 0)  C3DT= (2 . * ( (CR3-CR3LST) /T) )-CR3DTL 
IF(FLAG2.EQ.O)  C2DT* ( 2 . * ( (CR2-CR2LST) /T) ) -CR2DTL 
IF(FLAGl.EQ.O)  C1DT= (2 . * ( (CR1-CR1LST) /T) ) -CR1DTL 
IF  (N3 . EQ . NSW3 . AND . FLAG3 . EQ . 1 )  GO  TO  30 
IF  (N2 . EQ . NSW2 .AND. FLAG2 . EQ. 1)  GO  TO  20 
IF  (N1 . EQ. NSW1 . AND. FLAG1 . EQ . 1)  GO  TO  10 
IF  (FLAG3.EQ.1)  C3DT= (2 . * ( (CR3-CR3LST) /T) ) -CR3DTL 
IF  (FLAG2.EQ.1)  C2DT= (2 . * ( (CR2-CR2LST) /T) ) -CR2DTL 
IF  (FLAG1.EQ. 1)  C1DT= ( 2 . * ( (CR1-CR1LST) /T) ) -CR1DTL 
30  N3=N3-*-l 


20  N2=N2+1 

10  N1=N1+1 

CR3DTL=C3DT 

CR2DTL=C2DT 

CR1DTL=C1DT 

CR3  2  L=CR3  LST 

CR2  2  Lr=CR2  LST 

CR12L=CR1LST 

CR3LST=CR3 

CR2LST=CR2 

CR1LST=CR1 

SORT 

******************************************************  **** 

TERMINAL 
METHOD  RKSFX 

CONTRL  FINTIM=0 . 5 , DELT=0 . 00005 , DELS=0 . 00025 

SAVE  (SI)  0 . 0004 , X1DOT, X2DOT , X3 DOT , C1DT , C2DT, C3DT , . . . 

CR1DT , CR2 DT , CR3 DT, CR1 , CR2 , CR3 
SAVE  (S2)  0.001, Cl, CR1,RR1,C2,CR2,RR2,C3,CR3,RR3 
SAVE  (S3)  0.001, El, E2,E3 
PRINT  0.01,  E1,E2,E3 
GRAPH  (Gl/Sl , DE=TEK618 , PO=l) . . . 

CR1 (LE=6 . 0 , NI=13 , LO=- . 1 , SC= . 1 , UN= ' RAD ' ) , . . . 

C1DT (LE=3 , NI=4 , LO=-3 , SC=3 ,UN= 'RAD/SEC  1 ) , . . . 

CR1DT ( LE=3 , NI=4 , LO=-3 , PO=6 . 0 , SC=3 , UN= ' RAD/ SEC ' ) , . . . 

XI DOT (LE=3 , NI=4 , LO=-3 , SC=3 , AX=OMIT) 

GRAPH  (G2/S1 , DE=TEK618 , OV , PO— 1 ,3.25) . . . 

CR2 ( LE=6 . 0 , NI=13 , LO=- . 1 , SC= . 1 , UN= ' RAD ' ) , . . . 

C2DT ( LE=3 , NI=4 , LO=-3 , SC=3 , UN= ' RAD/SEC ' ) , . . . 

CR2DT (LE=3 , NI=4 , LO=-3 , PO=6 . 0 , SC=3 , UN= ' RAD/SEC  1 ) , . . . 

X2 DOT ( LE=3 , NI=4 , LO«-3 , SC=3 , AX=OMIT ) 

GRAPH  (G3/S1 , DE=TEK618 , OV , PO=l , 6.5) . . . 

CR3  (LE=6.0,NI=1  ,  ;,0=- .  1 ,  SC=.  1  ,UN= 'RAD' )  ,  .  .  . 

C3DT (LE=3 , NI=4  *  -3 , SC=8 , UN= • RAD/SEC ' ) , . . . 

CR3DT  (LE=3  ,  NI~  -  .  "  ';--8  ,  PO=6 . 0  ,  SC=8  ,UN=  '  RAD/SEC  1  )  ,  .  .  . 
X3DOT ( LE=3 , NI=4 , ^0=-8 , SC=8 , AX=OMIT) 

GRAPH  (G4/S2 , DE=TEK618 , PO=l)  TIME(LE=6.0,NI=5,UN='SEC'),.-- 
Cl (LE=3 , NI=4 , LO=- . 2 , UN= ' RAD ' , SC=0 . 4 , PO=6 . 0) , . . . 

CR1 ( LE=3 , NI=4 , LO=- . 2 , UN= ' RAD ' , SC=0 . 4 ) , . . . 

RR1 ( LE=3 , NI=4 , LO=- . 2 , SC=0 . 4 , AX=OMIT) 

GRAPH  (G5/S2 , DE=TEK618 , OV, PO=l ,3.25) . . . 

TIME ( LE=6 . 0 , NI=5 , UN= ' SEC) , . . . 

C2 (LE=3 , NI=4 , LO=-. 2 , UN= 1  RAD ' , SC=0 . 4 , PO=6 . 0) , . . . 

CR2 (LE=3 , NI=4 , LO=- . 2 , UN= ' RAD ' ,SC=0.4) , . . . 

RR2 ( LE=3 , NI=4 , LO=- . 2 , SC=0 . 4 , AX=OMIT) 

GRAPH  (G6/S2 , DE=TEK618 ,0V, PO=l , 6.5) . . . 

TIME ( LE=6 . 0 , NI=5 , UN= ' SEC 1 ) , . . . 

C3 ( LE=3 , NI=4 , LO=- . 2 , UN= ' RAD ' , SC=0 . 4 , PO=6 . 0) , . . . 

CR3 ( LE=3 , NI=4 , LO=- . 2 , UN= ' RAD 1 ,SC=0. 4) , . . . 
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RR3 ( LE=3 , NI=4 , L0=- . 2 , SC=0 . 4 , AX=OMIT) 

GRAPH  (G7/S3 , DE=TEK618 , P0=1 , . 5)  .  .  . 

TIME (LE=5.0,NI=5,UN=' SEC' ) ,  .  .  . 

El ( LE=8 , NI=7 , L0=- . 2 , UN= ' RAD ' ,SC=.2) , . . . 

E2 ( LE=8 , NI=7 , L0=- . 2 , UN= ' RAD ' , SC=. 2 , P0=5 . 0) , . . . 

E3 ( LE=8 , NI=7 , LO=- „ 2 , UN= ' RAD ' , SC=. 2 , PO=6 . 2 ) 
LABEL  (Gl)  PHASE  PLANE  ( CDT , CRDT , XDOT . VS . CR) 

LABEL  (G4)  STEP  RESPONSE 
LABEL  (G7 )  ERROR  .VS.  TIME 
END 
STOP 


* *********************************************************** 


DSL  PROGRAM  FOR  THE  THREE  LINK  REVOLUTE  ROBOT  WITH 
DISTURBANCE  (GRAVITATIONAL  TORQUES  INCLUDED) 


TITLE  MODEL  OF  REVOLUTE  3 -DEG  OF  FREEDOM  ROBOT  ARM 
PARAM  K=1 .  0 ,  K1=0 .6,K2=10000.0,  KM1=0 .422  5,  KM2=0 . 4225 ,  KM3=4 . 0 
PARAM  VSAT=150 . 0 , M1=0 . 268 ,M2=0 .227 ,  M3=0 .041 
PARAM  01=15.0,02=10.0,03=10.0,0=386.4 

PARAM  J1=0 . 033 , J2=0 .033 , J3=0 . 033 , R1=0 . 91 , R2=0 .91, R3=0 . 91 
PARAM  KT1=14 . 4 , KT2=14 . 4 , KT3=14 . 4 , Ll= . 0001 , L2= . 0001 , L3= .0001 
PARAM  BM1=0. 04297, BM2=0. 04297, BM3=0. 04297, LOAD=0.0 
PARAM  KV1=0. 1012, KV2=0. 1012, KV3=0. 1012, T=0. 00025 
PARAM  REF1=1 . 0  ,REF2=1. 0  ,REF3=1. 0 
INTGER  SW1,SW2,SW3,N1,N2,N3 

*  Kl:  THE  CURVE  SCALLING  CONSTANT 

*  K2 :  THE  AMPLIFIER  GAIN 

*  KM:  THE  INITIAL  CONSTANT  OF  THE  IDEAL  (MODEL)  MOTOR 

*  VSAT:  THE  SATURATION  LIMITS  OF  THE  AMPLIFIER 

*  K:  THE  'TELOCITY  LOOP  FEEDBACK  GAIN  (OF  THE  MODEL) 

*  REF1 , REF2 , REF3 : THE  COMMANDED  TIP  POSITION  IN  RADS 

*  T:  THE  SAMPLING  INTERVAL 
INITIAL 

FLAG 1=0 

FLAG2=0 

FLAG3=0 

N1=0 

N2=0 

N3=0 

X1DOT=0 . 

X2DOT=0. 

X3DOT=0 . 

C1=0. 

C2=0 . 

C3=0 . 

C1DT=0 . 

C2DT=0 . 

C3DT=0. 

C1DDT=0 . 

C2DDT=0 . 

C3DDT=0 . 

CR1=0. 

CR2=0 . 

CR3=0 . 

CR1DT=0. 

CR2DT=0. 

CR3DT=0. 

CR1DDT=0 . 


CR2DDT=0 . 

CR3DDT=0. 

TL1=0. 

TL2=0. 

TL3=0. 

MP1=0. 

MP2=0 . 

MP3=0. 

MT1=0. 

MT2=0 . 

MT3=0. 

A1=SQRT ( 2 . *KM1*VSAT) 

A2=SQRT ( 2 . *KM2 *VSAT) 

A3=SQRT ( 2 . *KM3 *VSAT) 

DERIVATIVE 

RR1=REF1*STEP(0 . 0) +50* (STEP( . 090) -STEP ( . 100) ) 

RR2=REF2*STEP(0 . 0) +50* (STEP ( . 090) -STEP ( . 100) ) 

RR3=REF3*STEP(0. 0) +50* (STEP( . 090) -STEP( . 100) ) 

E1=RR1-C1 

E2=RR2-C2 

E3=RR3-C3 


NOSORT 


Dll  =  M3* (D2*COS (CR2) +D3*COS (CR2+CR3 ) ) **2 . . . 

+M2*D2**2* (COS (CR2) ) **2 
D112=  2* ( (M2+M3) *D2**2*COS (CR2) *SIN (CR2) . . . 

+M3*D3**2*COS (CR2+CR3) *SIN (CR2+CR3) ... 
+M3*D2*D3*SIN (2*CR2+CR3 ) ) 

D113=  2* (M3*D3**2*COS (CR2+CR3) *SIN (CR2+CR3) . . . 

+M3  *D2  *D3  *COS ( CR2 ) *SIN ( CR2+CR3 ) ) 

D22  =  (M2+M3) *D2**2+M3*D3**2+2*M3*D2*D3*SIN (CR3) 
D23  =  M3*D3**2+M3*D2*D3*SIN (CR3) 

D211=  (M2+M3) *D2**2*COS (CR2) *SIN(CR2) . . . 

+M3*D3**2*COS (CR2+CR3 ) *SIN (CR2+CR3 ) . . . 
+M3*D2*D3*SIN (2*CR2+CR3 ) 

D223=  2*M3*D2*D3*SIN (CR3 ) 

D233=  M3*D2*D3*SIN (CR3 ) 

D32  =  M3*D3**2+M3*D2*D3*COS (CR3) 

D33  =  M3*D3**2 

D311=  M3*D3**2*COS (CR2+CR3) *SIN (CR2+CR3) . . . 

+M3*D2*D3*COS (CR2) *SIN (CR2+CR3) 

D322=  M3*D2*D3*SIN (CR3) 

G2  =  (M2+M3) *G*D2*COS (CR2) +M3*G*D3*COS (CR2+CR3) 
G3  =  M3*G*D3*COS (CR2+CR3) 

TL1  =  -D112  *CR1DT*CR2DT-D113  *CR1DT*CR3DT 
TL2  =  D23*CR3DDT+D211*CR1DT**2-D233*CR3DT**2 . . . 
-D223  *CR2DT*CR3DT+G2 

TL3  =  DJ  2  *CR2DDT+D3 11*CR1DT**2+D322  *CR2DT**2+G3 
JTOT1  =  Dll + J 1 
JTOT2  =  D22+J2 
JTOT3  =  D3  i  *  .J  2 

IF (El . LT. 0.0)  X1D0T=-A1*K1*SQRT (ABS (El) ) 


IF (El . GE. 0.0)  X1D0T=A1*K1*SQRT(E1) 

IF (E2 . LT. 0 . 0)  X2D0T=-A2*K1*SQRT(ABS(E2) ) 

IF (E2 . GE . 0 . 0)  X2DOT=A2  *K1*SQRT (E2 ) 

IF (E3 . LT. 0.0)  X3D0T=-A3*K1*SQRT(ABS(E3) ) 

IF (E3 . GE . 0 . 0 )  X3 DOT=A3  *K1*SQRT (E3 ) 

SORT 

************************************************************ 

KC1D0T=K*C1DT 

X1D0TE=X1D0T-KC1D0T 

V1=LIMIT ( -VS AT , VSAT , K2  *X1D0TE ) 

C1DDT=V1*KM1 

NOSORT 

IF  (FLAG1.EQ. 1)  GO  TO  2 

IF  ( VI. LT. VSAT. AND. TIME. GT. 0.00005)  FLAG1=1 
NSW1=N1 

2  CONTINUE 

SORT 

C 1 DT= I NTGRL (0.0, C1DDT) 

C1=INTGRL( 0 . 0 , C1DT) 

VM1=V1-KV1*CR1DT 
MP1=REALPL (0.0, Ll/Rl , VM1/R1 ) 

MT1— KT1*MP1 

MT1E=MT1“BM1*CR1DT-TL1 
CR1DDT= ( 1 . / JTOT1 ) *MT1E 
CR1DT=INTGRL (0.0, CR1DDT) 

CR1«INTGRL (0.0, CR1DT) 

************************************************************ 

KC2DOT=K*C2DT 

X2 DOTE=X2 DOT-KC2 DOT 

V2 =LIMIT ( -VSAT , VSAT , K2  *X2 DOTE ) 

NOSORT 

IF  ( FLAG 2 . EQ . 1 )  GO  TO  4 

IF  (V2. LT. VSAT. AND. TIME. GT. 0.00005)  FLAG2=1 
NSW2=N2 

4  CONTINUE 

SORT 

C2DDT=V2*KM2 
C2 DT=INTGRL (0.0, C2  DDT ) 

C2=INTGRL (0 . 0 , C2DT) 

VM2=V2 -KV2 *CR2 DT 
MP2=REALPL (0.0, L2/R2 , VM2/R2 ) 

MT2=KT2*MP2 

MT2E=MT2 -BM2  *CR2  DT-TL2 
CR2DDT= ( 1 . / JTOT2 ) *MT2E 
CR2 DT=INTGRL (0.0, CR2 DDT ) 

CR2=INTGRL (0.0, CR2 DT ) 


I 


KC3  DOT— K*C3  DT 

X3 DOTE=X3 DOT-KC3 DOT 

V3=LIMIT ( -VS AT , VSAT , K2  *X3  DOTE ) 


> 


NOSORT 


6 

SORT 


* 


IF  (FLAG3.EQ.1)  GO  TO  6 

IF  ( V3.LT. VSAT. AND. TIME. GT. 0.00005)  FLAG3=1 

NSW3=N3 

CONTINUE 

C3DDT=V3  *KM3 
C3 DT=INTGRL ( 0 . 0 , C3 DDT  ’ 

C3=INTGRL(0 . 0, C3DT) 

VM3=V3 -KV3  *CR3  DT 
MP3=REALPL (0.0, L3/R3 , VM3/R3 ) 

MT3=KT3*MP3 

MT3E=MT3-BM3*CR3DT-TL3 
CR3 DDT- ( 1 . / JTOT3 ) *MT3  E 
CR3 DT=INTGRL (0.0, CR3 DDT ) 

CR3=INTGRL (0.0, CR3  DT) 


a*********************************************************** 

SAMPLE 

NOSORT 

IF  (N3.EQ.0)  GO  TO  30 

IF  (N2.EQ.0)  GO  TO  20 

IF  (N3.EQ.0)  GO  TO  10 

C3=CR3 

C2=CR2 

C1=CR1 

KS3=ABS ( 2 . *CR3 ) / ( ( (N3*T) **2) *V3) 

KS2=ABS ( 2 . *CR2 ) / ( ( (N2*T) **2 ) *V2 ) 

KSl=ABS(2.*CRl)/( ( (N1*T) **2) *V1) 

IF  (FLAG3.EQ.0)  KM3=KS3 
IF  (FLAG2.EQ.0)  KM2=KS2 
IF  (FLAG1.EQ. 0)  KM1=KS1 
IF  (N3.GE.2)  CR3DTL= (CR3-CR32L) / (2 . *T) 

IF  (N2.GE.2)  CR2  DTL= ( CR2 -CR2  2L)/(2.*T) 

IF  (N1.GE.2)  CR1DTL= (CR1-CR12L) / (2 . *T) 

IF(FLAG3.EQ.O)  C3DT= ( 2 . * ( (CR3-CR3LST) /T) ) -CR3DTL 
IF (FLAG2 . EQ. 0)  C2DT= (2 . * ( (CR2-CR2LST) /T) )-CR2DTL 
IF (FLAG1 . EQ. 0)  C1DT=(2.*( (CR1-CR1LST) /T) ) -CR1DTL 
IF  (N3 . EQ.NSW3 . AND. FLAG3 . EQ. 1)  GO  TO  30 
IF  (N2.EQ.NSW2 .AND. FLAG 2 .EQ. 1)  GO  TO  20 
IF  (N1 . EQ.NSW1 . AND. FLAG1 . EQ. 1)  GO  TO  10 
IF  (FLAG3.EQ.1)  C3DT= ( 2 . * ( (CR3-CR3LST) /T) ) -CR3DTL 
IF  (FLAG2.EQ.1)  C2DT= (2 . * ( (CR2-CR2LST) /T) ) -CR2DTL 
IF  (FLAG1.EQ. 1)  C1DT= ( 2 . * ( (CR1-CR1LST) /T) ) -CR1DTL 
30  N3=N3+1 

20  N2=N2+1 

10  N1=N1+1 
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SORT 


CR3DTL=C3DT 
CR2DTLr=C2DT 
CR1DTL=C1DT 
CR3  2  L=CR3  LST 
CR2  2  L=CR2  LST 
CR12L=CR1LST 
CR3LST=CR3 
CR2LST=CR2 
CR1LST— CR1 


************************************************************ 

TERMINAL 
METHOD  RKSFX 

CONTRL  FINTIM=0 . 5 , DELT=0 . 00005 , DELS=0 . 00025 

SAVE  (SI)  0 . 0004 , X1DOT, X2DOT , X3DOT, C1DT, C2DT, C3DT, . . . 

CR1DT, CR2DT, CR3DT, CR1 , CR2 , CR3 
SAVE  (S2)  0.001, Cl, CR1 , RR1 , C2 , CR2 , RR2 , C3 , CR3 , RR3 
GRAPH  (Gl/Sl , DE=TEK618 , PO=l) . .. 

CR1 ( LE=6 . 0 , NI=13 , LO=- . 1 , SC= . 1 , UN= ' RAD 1 ) , . . . 

C1DT (LE=3 , NI=4 , LO=-3 , SC=3 , UN= ' RAD/SEC ' ) , . . . 

CR1DT (LE=3 ,NI=4 , LO=-3 , PO=6 . 0, SC=3 , UN= ' RAD/SEC ' ) , . . . 

XI DOT (LE=3 , NI=4 , LO=-3 , SC=3 , AX-OMIT) 

GRAPH  (G2/S1 , DE=TEK618 , OV, PO=l ,3.25) . . . 

CR2 (LE=6 . 0 , NI=13 , LO=- . 1 , SC=. 1 , UN= ' RAD ' ) , . . . 
C2DT(LE=3,NI=4,LO=-3,SC=3,UN=' RAD/SEC ' ) , . . . 

CR2DT (LE=3 , NI=4 , LO=-3 , PO=6 . 0 , SC=3 , UN= ' RAD/ SEC ' ) , . . . 
X2DOT(LE— 3 , NI=4 , LO=-3 , SC=3 , AX=OMIT) 

GRAPH  (G3/S1 , DE=TEK618 , OV, PO— 1 ,6.5) . . . 

CR3 ( LE=6 . 0 , NI=13 , LO=- . 1 , SC= . 1 , UN= ' RAD ' ) , . . . 

C3DT ( LE=3 , NI=4 , LO=-8 , SC=8 , UN= ' RAD/SEC ' ) , . . . 

CR3DT (LE=3 ,NI=4 , LO=-8 , PO=6 . 0 , SC=8 ,UN= ' RAD/SEC ' ) , . . . 

X3  DOT ( LE=3 , NI=4 , LO=-8 , SC=8 , AX=OMIT) 

GRAPH  (G4/S2 , DE=TEK618 , PO=l)  TIME ( LE=6 . 0 , NI=5 , UN= ' SEC '),... 
Cl ( LE=3 , NI=4 , LO=- . 2 , UN= ' RAD ' , SC=0 . 4 , PO=6 . 0) , . . . 

CR1 ( LE=3 , NI=4 , LO=- . 2 , UN= ' RAD ' ,SC=0.4) , . . . 

RR1 (LE=3 , NI=4 , LO=- . 2 , SC=0 . 4 , AX=OMIT) 

GRAPH  (G5/S2 , DE— TEK618 , OV, PO=l , 3.25) . . . 

TIME (LE=6 . 0 , NI=5 , UN= 'SEC  1 ) , . . . 

C2 ( LE=3 , NI=4 , LO=- . 2 , UN= ' RAD ' , SC=0 . 4 , PO=6 . 0) , . . . 

CR2 ( LE=3 , NI=4 , LO=- . 2 , UN- ' RAD ' ,SC=0.4) , . . . 

RR2 ( LE=3 , NI=4 , LO=- . 2 , SC=0 . 4 , AX=OMIT) 

GRAPH  (G6/S2 , DE=TEK618 ,0V, PO=l, 6.5) . . . 

TIME (LE=6.0,NI=5,UN=' SEC') , . . . 

C3 ( LE=3 , NI=4 , LO=- . 2 , UN= ' RAD ' , SC=0 . 4 , PO=6 . 0) , . . . 

CR3 ( LE=3 , NI=4 , LO=- . 2 , UN= ' RAD ' ,SC=0.4) , . . . 

RR3 (LE=3 , NI=4 , LO=- . 2 , SC=0 . 4 , AX=OMIT) 

LABEL  (Gl)  PHASE  PLANE  ( CDT , CRDT , XDOT . VS . CR) 

LABEL  (G4 )  STEP  RESPONSE 

END 

STOP 
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